diff --git a/.gitignore b/.gitignore index f7cd906a..c59e1c00 100644 --- a/.gitignore +++ b/.gitignore @@ -52,6 +52,7 @@ coverage.xml # Sphinx documentation docs/_build/ +doc/ # PyBuilder target/ @@ -83,3 +84,9 @@ gradle-app.setting # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 # gradle/wrapper/gradle-wrapper.properties + +# Eclipse +*.launch +.project +.cproject +.pydevproject diff --git a/.travis.yml b/.travis.yml index 00c86f93..35b298da 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,22 +7,36 @@ services: before_install: - docker login -e="$DOCKER_EMAIL" -u="$DOCKER_USERNAME" -p="$DOCKER_PASSWORD" -- docker pull esteve/ros2-ubuntu-xenial-travisci:java +- docker pull theosakamg7/ros2java:latest - cd /home/travis/build - mkdir -p ament_ws/src - cd /home/travis/build/ament_ws -- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` esteve/ros2-ubuntu-xenial-travisci:java sh -c "/usr/bin/wget https://raw.githubusercontent.com/esteve/ament_java/master/ament_java.repos" -- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` esteve/ros2-ubuntu-xenial-travisci:java sh -c "/usr/bin/vcs import src < ament_java.repos" -- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` esteve/ros2-ubuntu-xenial-travisci:java sh -c "src/ament/ament_tools/scripts/ament.py build --symlink-install --isolated" +- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` theosakamg7/ros2java:latest sh -c "/usr/bin/wget https://gist.githubusercontent.com/Theosakamg/e6084cfafa6b7ea690104424cef970a2/raw/ament_java.repos" +- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` theosakamg7/ros2java:latest sh -c "/usr/bin/vcs import src < ament_java.repos" +- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` theosakamg7/ros2java:latest sh -c "src/ament/ament_tools/scripts/ament.py build --symlink-install --isolated" - cd /home/travis/build - mkdir -p ros2_java_ws/src - cd /home/travis/build/ros2_java_ws -- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` esteve/ros2-ubuntu-xenial-travisci:java sh -c "/usr/bin/wget https://raw.githubusercontent.com/esteve/ros2_java/master/ros2_java_desktop.repos" -- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` esteve/ros2-ubuntu-xenial-travisci:java sh -c "/usr/bin/vcs import src < ros2_java_desktop.repos" +- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` theosakamg7/ros2java:latest sh -c "/usr/bin/wget https://gist.githubusercontent.com/Theosakamg/d9259bbc708c5145255fbdeb25e65e19/raw/ros2_java_desktop.repos" +- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` theosakamg7/ros2java:latest sh -c "/usr/bin/vcs import src < ros2_java_desktop.repos" - rm -rf /home/travis/build/ros2_java_ws/src/ros2_java/ros2_java -- ln -s /home/travis/build/esteve/ros2_java /home/travis/build/ros2_java_ws/src/ros2_java/ros2_java +- ln -s /home/travis/build/ros2java-alfred/ros2_java + /home/travis/build/ros2_java_ws/src/ros2_java/ros2_java +- cd /home/travis/build/ros2_java_ws/src/ros2/rosidl_typesupport +- patch -p1 < ../../ros2_java/ros2_java/rosidl_ros2_java.diff - cd /home/travis/build -- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` esteve/ros2-ubuntu-xenial-travisci:java sh -c ". ament_ws/install_isolated/local_setup.sh && cd /home/travis/build/ros2_java_ws && ament build --symlink-install --isolated" +- docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` theosakamg7/ros2java:latest sh -c ". ament_ws/install_isolated/local_setup.sh && cd /home/travis/build/ros2_java_ws && ament build --symlink-install --isolated" script: -- cd /home/travis/build && docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` esteve/ros2-ubuntu-xenial-travisci:java sh -c ". ament_ws/install_isolated/local_setup.sh && cd /home/travis/build/ros2_java_ws && ament test --isolated --only-packages ament_cmake_export_jars rcljava rcljava_common rosidl_generator_java" +- cd /home/travis/build && docker run -u "$UID" -it --rm -v `pwd`:`pwd` -w `pwd` theosakamg7/ros2java:latest sh -c ". ament_ws/install_isolated/local_setup.sh && cd /home/travis/build/ros2_java_ws && ament test --isolated --only-packages ament_cmake_export_jars rcljava rcljava_common rosidl_generator_java" + +after_success: + - coveralls + +notifications: + webhooks: + urls: + - https://webhooks.gitter.im/e/4aac82b42245203edceb + on_success: change # options: [always|never|change] default: always + on_failure: always # options: [always|never|change] default: always + on_start: never # options: [always|never|change] default: always diff --git a/AUTHORS b/AUTHORS index eccd2baf..a70caa40 100644 --- a/AUTHORS +++ b/AUTHORS @@ -1 +1,2 @@ Esteve Fernandez https://github.com/esteve +Mickael Gaillard https://github.com/Theosakamg diff --git a/LICENSE b/LICENSE index 5de4dc8e..443791df 100644 --- a/LICENSE +++ b/LICENSE @@ -177,6 +177,7 @@ END OF TERMS AND CONDITIONS Copyright 2016 Esteve Fernandez + Copyright 2016 Mickael Gaillard Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/README.md b/README.md index 575d675f..5c02adea 100644 --- a/README.md +++ b/README.md @@ -1,191 +1,18 @@ -ROS2 for Java +ROS2 for Java (Alfred variation) +[![Build Status](https://travis-ci.org/ros2java-alfred/ros2_java.svg?branch=master)](https://travis-ci.org/ros2java-alfred/ros2_java) [![Coverage Status](https://coveralls.io/repos/github/ros2java-alfred/ros2_java/badge.svg?branch=master)](https://coveralls.io/github/ros2java-alfred/ros2_java?branch=master) [![Join the chat at https://gitter.im/ros2java-alfred](https://badges.gitter.im/gitterHQ/gitterHQ.github.io.svg)](https://gitter.im/ros2java-alfred?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) Coverity Scan Build Status ============= Build status ------------ -**JRE** [![Build Status](https://travis-ci.org/esteve/ros2_java.svg?branch=master)](https://travis-ci.org/esteve/ros2_java) +**JRE** [![Build Status](https://travis-ci.org/ros2java-alfred/ros2_java.svg?branch=master)](https://travis-ci.org/ros2java-alfred/ros2_java) -**Android** [![Build Status](https://travis-ci.org/esteve/ros2_android.svg?branch=master)](https://travis-ci.org/esteve/ros2_android) +**Android** [![Build Status](https://travis-ci.org/ros2java-alfred/ros2_android.svg?branch=master)](https://travis-ci.org/ros2java-alfred/ros2_android) What is this? ------------- This is a set of projects (bindings, code generator, examples and more) that enables developers to write ROS2 -applications for the JVM and Android. +applications for the JVM and Android with design of rosjava (ROS1). -Besides this repository itself, there's also: -- https://github.com/esteve/ament_java, which adds support for Gradle to Ament -- https://github.com/esteve/ament_gradle_plugin, a Gradle plugin that makes it easier to use ROS2 in Java and Android project, and which can be found published at the Gradle Central at https://plugins.gradle.org/plugin/org.ros2.tools.gradle -- https://github.com/esteve/ros2_java_examples, examples for the Java Runtime Environment -- https://github.com/esteve/ros2_android_examples, examples for Android - -Is this Java only? ------------------- - -No, any language that targets the JVM can be used to write ROS2 applications. - -Including Android? ------------------- - -Yep! Make sure to use [this fork](https://github.com/eProsima/Fast-RTPS/pull/26) as your DDS vendor. - -Features --------- - -The current set of features include: -- Generation of all builtin and complex ROS types, including arrays, strings, nested types, constants, etc. -- Support for publishers and subscriptions -- Tunable Quality of Service (e.g. lossy networks, reliable delivery, etc.) -- Clients and services -- Support for Android - -Sounds great, how can I try this out? -------------------------------------- - -First of all, download the ament repositories in a separate workspace: - -``` -mkdir -p ~/ament_ws/src -cd ~/ament_ws -wget https://raw.githubusercontent.com/esteve/ament_java/master/ament_java.repos -vcs import ~/ament_ws/src < ament_java.repos -src/ament/ament_tools/scripts/ament.py build --symlink-install --isolated -``` - -You may wonder why this is needed if the ROS2 instructions already fetch Ament on the same workspace as ROS2. - -The reason is that this includes an additional build type for Gradle projects, and you'll need Ament to pick it up so it can build the examples, so this has to happen in a separate step. - -The following sections deal with building the `ros2_java` codebase for the desktop Java runtime and for Android. - -Desktop -------- - -``` -mkdir -p ~/ros2_java_ws/src -cd ~/ros2_java_ws -wget https://raw.githubusercontent.com/esteve/ros2_java/master/ros2_java_desktop.repos -vcs import ~/ros2_java_ws/src < ros2_java_desktop.repos -cd ~/ros2_java_ws -. ~/ament_ws/install_isolated/local_setup.sh -ament build --symlink-install --isolated -``` - -Now you can just run a couple of examples. - -Talker and Listener -------------------- - -Talker: - -``` -. ~/ament_ws/install_isolated/local_setup.sh -. ~/ros2_java_ws/install_isolated/local_setup.sh -cd ~/ros2_java_ws -java -cp install_isolated/rcljava_common/share/rcljava_common/java/slf4j-jdk14-1.7.21.jar:install_isolated/rcljava_common/share/rcljava_common/java/slf4j-api-1.7.21.jar:install_isolated/std_msgs/share/std_msgs/java/std_msgs.jar:install_isolated/rcljava/share/rcljava/java/rcljava.jar:install_isolated/rcljava_examples/share/rcljava_examples/java/rcljava_examples.jar:install_isolated/example_interfaces/share/example_interfaces/java/example_interfaces.jar:install_isolated/rcljava_common/share/rcljava_common/java/rcljava_common.jar org.ros2.rcljava.examples.Talker -``` - -Listener: - -``` -. ~/ament_ws/install_isolated/local_setup.sh -. ~/ros2_java_ws/install_isolated/local_setup.sh -cd ~/ros2_java_ws -java -cp install_isolated/rcljava_common/share/rcljava_common/java/slf4j-jdk14-1.7.21.jar:install_isolated/rcljava_common/share/rcljava_common/java/slf4j-api-1.7.21.jar:install_isolated/std_msgs/share/std_msgs/java/std_msgs.jar:install_isolated/rcljava/share/rcljava/java/rcljava.jar:install_isolated/rcljava_examples/share/rcljava_examples/java/rcljava_examples.jar:install_isolated/example_interfaces/share/example_interfaces/java/example_interfaces.jar:install_isolated/rcljava_common/share/rcljava_common/java/rcljava_common.jar org.ros2.rcljava.examples.Listener -``` - -Client and Service ------------------- - -Client: - -``` -. ~/ament_ws/install_isolated/local_setup.sh -. ~/ros2_java_ws/install_isolated/local_setup.sh -cd ~/ros2_java_ws -java -cp install_isolated/rcljava_common/share/rcljava_common/java/slf4j-jdk14-1.7.21.jar:install_isolated/rcljava_common/share/rcljava_common/java/slf4j-api-1.7.21.jar:install_isolated/std_msgs/share/std_msgs/java/std_msgs.jar:install_isolated/rcljava/share/rcljava/java/rcljava.jar:install_isolated/rcljava_examples/share/rcljava_examples/java/rcljava_examples.jar:install_isolated/example_interfaces/share/example_interfaces/java/example_interfaces.jar:install_isolated/rcljava_common/share/rcljava_common/java/rcljava_common.jar org.ros2.rcljava.examples.AddTwoIntsClient -``` - -Service: - -``` -. ~/ament_ws/install_isolated/local_setup.sh -. ~/ros2_java_ws/install_isolated/local_setup.sh -cd ~/ros2_java_ws -java -cp install_isolated/rcljava_common/share/rcljava_common/java/slf4j-jdk14-1.7.21.jar:install_isolated/rcljava_common/share/rcljava_common/java/slf4j-api-1.7.21.jar:install_isolated/std_msgs/share/std_msgs/java/std_msgs.jar:install_isolated/rcljava/share/rcljava/java/rcljava.jar:install_isolated/rcljava_examples/share/rcljava_examples/java/rcljava_examples.jar:install_isolated/example_interfaces/share/example_interfaces/java/example_interfaces.jar:install_isolated/rcljava_common/share/rcljava_common/java/rcljava_common.jar org.ros2.rcljava.examples.AddTwoIntsService -``` - -You can also combine any scenario where the talker/listener or client/service are written in Java, Python and C++ and they should talk to each other. - -Android -------- - -The Android setup is slightly more complex, you'll need the SDK and NDK installed, and an Android device where you can run the examples. - -Make sure to download at least the SDK for Android Lollipop (or greater), the examples require the API level 21 at least. - -You may download the Android NDK from [the official](https://developer.android.com/ndk/downloads/index.html) website, let's assume you unpack it to `~/android_ndk` - -``` -mkdir -p ~/ros2_android_ws/src -cd ~/ros2_android_ws -wget https://raw.githubusercontent.com/esteve/ros2_java/master/ros2_java_android.repos -vcs import ~/ros2_android_ws/src < ros2_java_android.repos -cd ~/ros2_android_ws/src/ros2/rosidl -touch python_cmake_module/AMENT_IGNORE -touch rosidl_generator_py/AMENT_IGNORE -cd ~/ros2_android_ws/src/eProsima/Fast-RTPS -git submodule init -git submodule update -cd ~/ros2_android_ws -. ~/ament_ws/install_isolated/local_setup.sh -ament build --isolated --cmake-args \ - -DPYTHON_EXECUTABLE=/usr/bin/python3 -DCMAKE_TOOLCHAIN_FILE=$HOME/android_ndk/android-ndk-r13b/build/cmake/android.toolchain.cmake \ - -DANDROID_FUNCTION_LEVEL_LINKING=OFF -DANDROID_NATIVE_API_LEVEL=android-21 \ - -DANDROID_TOOLCHAIN_NAME=arm-linux-androideabi-clang -DANDROID_STL=gnustl_shared \ - -DANDROID_ABI=armeabi-v7a -DANDROID_NDK=$HOME/android_ndk/android-ndk-r13b -DTHIRDPARTY=ON -DCOMPILE_EXAMPLES=OFF -DCMAKE_FIND_ROOT_PATH="$HOME/ament_ws/install_isolated;$HOME/ros2_android_ws/install_isolated" \ - -DANDROID_CPP_FEATURES="exceptions rtti" -- \ - --ament-cmake-args \ - -DPYTHON_EXECUTABLE=/usr/bin/python3 -DCMAKE_TOOLCHAIN_FILE=$HOME/android_ndk/android-ndk-r13b/build/cmake/android.toolchain.cmake \ - -DANDROID_FUNCTION_LEVEL_LINKING=OFF -DANDROID_NATIVE_API_LEVEL=android-21 \ - -DANDROID_TOOLCHAIN_NAME=arm-linux-androideabi-clang -DANDROID_STL=gnustl_shared \ - -DANDROID_ABI=armeabi-v7a -DANDROID_NDK=$HOME/android_ndk/android-ndk-r13b -DTHIRDPARTY=ON -DCOMPILE_EXAMPLES=OFF -DCMAKE_FIND_ROOT_PATH="$HOME/ament_ws/install_isolated;$HOME/ros2_android_ws/install_isolated" \ - -DANDROID_CPP_FEATURES="exceptions rtti" -- \ - --ament-gradle-args \ - -Pament.android_stl=gnustl_shared -Pament.android_abi=armeabi-v7a -Pament.android_ndk=$HOME/android_ndk/android-ndk-r13b -- -``` - -The talker and listener example Android apps can be installed via adb, plug your Android device to your computer with a USB cable and type the following: - -Talker: - -``` -adb install ~/ros2_android_ws/install_isolated/ros2_talker_android/ros2_talker_android-debug.apk -``` - -Listener: - -``` -adb install ~/ros2_android_ws/install_isolated/ros2_listener_android/ros2_listener_android-debug.apk -``` - -You can try out running the talker on the desktop and the listener on your Android device or viceversa. - -Enjoy! - -Acknowledgements ----------------- - -Thanks all those who have contributed: - -Mickael Gaillard (https://github.com/Theosakamg) - -TODO ----- - -There's a bunch of features missing, including efficient intraprocess communication and DDS domain separation. - -Large messages would benefit from Java's NIO. - -And of course, this wouldn't be a proper opensource project if it didn't lack tests and documentation, so there's that too. +To start => [Wiki](https://github.com/ros2java-alfred/ros2_java/wiki) diff --git a/common_interfaces_full.diff b/common_interfaces_full.diff new file mode 100644 index 00000000..67cb1109 --- /dev/null +++ b/common_interfaces_full.diff @@ -0,0 +1,35 @@ +diff --git a/sensor_msgs/CMakeLists.txt b/sensor_msgs/CMakeLists.txt +index 5d85e46..5ce78ba 100644 +--- a/sensor_msgs/CMakeLists.txt ++++ b/sensor_msgs/CMakeLists.txt +@@ -20,8 +20,8 @@ set(msg_files + "msg/CameraInfo.msg" + "msg/ChannelFloat32.msg" + "msg/CompressedImage.msg" +- # "msg/FluidPressure.msg" +- # "msg/Illuminance.msg" ++ "msg/FluidPressure.msg" ++ "msg/Illuminance.msg" + "msg/Image.msg" + "msg/Imu.msg" + # "msg/JointState.msg" +@@ -30,7 +30,7 @@ set(msg_files + # "msg/JoyFeedbackArray.msg" + # "msg/LaserEcho.msg" + "msg/LaserScan.msg" +- # "msg/MagneticField.msg" ++ "msg/MagneticField.msg" + # "msg/MultiDOFJointState.msg" + # "msg/MultiEchoLaserScan.msg" + # "msg/NavSatFix.msg" +@@ -40,8 +40,8 @@ set(msg_files + "msg/PointField.msg" + # "msg/Range.msg" + "msg/RegionOfInterest.msg" +- # "msg/RelativeHumidity.msg" +- # "msg/Temperature.msg" ++ "msg/RelativeHumidity.msg" ++ "msg/Temperature.msg" + # "msg/TimeReference.msg" + ) + set(srv_files diff --git a/rcljava/.gitignore b/rcljava/.gitignore new file mode 100644 index 00000000..a1d63343 --- /dev/null +++ b/rcljava/.gitignore @@ -0,0 +1,2 @@ +/bin/ +.classpath diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 302b044b..c33e077e 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -9,18 +9,31 @@ find_package(rmw REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(rcljava_common REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(builtin_interfaces REQUIRED) + include(CrossCompilingExtra) + if(ANDROID) - find_host_package(Java COMPONENTS Development) + # https://cmake.org/cmake/help/v3.0/module/FindJava.html?highlight=java + find_host_package(Java 1.6 COMPONENTS Development) else() - find_package(Java COMPONENTS Development) + # https://cmake.org/cmake/help/v3.0/module/FindJava.html?highlight=java + find_package(Java 1.6 COMPONENTS Development) + + # https://cmake.org/cmake/help/v3.0/module/FindJNI.html?highlight=jni find_package(JNI REQUIRED) endif() +# https://cmake.org/cmake/help/v3.0/module/UseJava.html?highlight=java include(UseJava) include(JavaExtra) +enable_testing() + +### Definitions ### set(CMAKE_JAVA_COMPILE_FLAGS "-source" "1.6" "-target" "1.6") if(NOT WIN32) @@ -29,23 +42,94 @@ endif() include_directories(include) +find_jar(JUNIT_JAR NAMES junit PATHS /usr/share/java VERSIONS 4) +find_jar(LOG_JAR NAMES commons-logging PATHS /usr/share/java) +find_jar(STD_MSGS_JARS NAMES std_msgs + PATHS "${CMAKE_BINARY_DIR}/../std_msgs/") +find_jar(RCLINTERFACE_MSGS_JARS NAMES rcl_interfaces + PATHS "${CMAKE_BINARY_DIR}/../rcl_interfaces/") +find_jar(BUILTIN_MSGS_JARS NAMES builtin_interfaces + PATHS "${CMAKE_BINARY_DIR}/../builtin_interfaces/") +find_jar(BUILTIN_COMMON_JARS NAMES rosidl_generator_java + PATHS "${CMAKE_BINARY_DIR}/../rosjava_common/") + +set(STD_MSGS_JARS "${CMAKE_BINARY_DIR}/../std_msgs/std_msgs.jar") +set(BUILTIN_MSGS_JARS "${CMAKE_BINARY_DIR}/../builtin_interfaces/builtin_interfaces.jar") +set(RCLINTERFACE_MSGS_JARS NAMES "${CMAKE_BINARY_DIR}/../rcl_interfaces/rcl_interfaces.jar") +set(BUILTIN_COMMON_JARS NAMES "${CMAKE_BINARY_DIR}/../rosjava_common/rosjava_common.jar") + +# Bug in find_jar for Junit4 : https://sourceforge.net/p/rdkit/mailman/message/35301727/ +set(JUNIT_JAR "/usr/share/java/junit4.jar") + +set(${PROJECT_NAME}_sources + "src/main/java/org/ros2/rcljava/Log.java" + "src/main/java/org/ros2/rcljava/NativeUtils.java" + "src/main/java/org/ros2/rcljava/RCLJava.java" + + "src/main/java/org/ros2/rcljava/internal/IClient.java" + "src/main/java/org/ros2/rcljava/internal/IGraph.java" + "src/main/java/org/ros2/rcljava/internal/INode.java" + "src/main/java/org/ros2/rcljava/internal/IPublisher.java" + "src/main/java/org/ros2/rcljava/internal/IService.java" + "src/main/java/org/ros2/rcljava/internal/ISubscription.java" + + "src/main/java/org/ros2/rcljava/namespace/GraphName.java" + + "src/main/java/org/ros2/rcljava/node/Node.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/QoSPolicy.java" + "src/main/java/org/ros2/rcljava/qos/policies/Reliability.java" + "src/main/java/org/ros2/rcljava/qos/QoSProfile.java" + + "src/main/java/org/ros2/rcljava/node/topic/Consumer.java" + "src/main/java/org/ros2/rcljava/node/topic/Publisher.java" + "src/main/java/org/ros2/rcljava/node/topic/Subscription.java" + "src/main/java/org/ros2/rcljava/node/topic/Topics.java" + + "src/main/java/org/ros2/rcljava/node/service/BiConsumer.java" + "src/main/java/org/ros2/rcljava/node/service/Client.java" + "src/main/java/org/ros2/rcljava/node/service/RCLFuture.java" + "src/main/java/org/ros2/rcljava/node/service/RMWRequestId.java" + "src/main/java/org/ros2/rcljava/node/service/Service.java" + "src/main/java/org/ros2/rcljava/node/service/TriConsumer.java" + + "src/main/java/org/ros2/rcljava/node/parameter/ParameterConsumer.java" + "src/main/java/org/ros2/rcljava/node/parameter/ParameterEventConsumer.java" + "src/main/java/org/ros2/rcljava/node/parameter/ParameterService.java" + "src/main/java/org/ros2/rcljava/node/parameter/ParameterVariant.java" + "src/main/java/org/ros2/rcljava/node/parameter/SyncParametersClient.java" + + "src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java" + "src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java" + "src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java" + "src/main/java/org/ros2/rcljava/exception/NotImplementedException.java" + "src/main/java/org/ros2/rcljava/exception/NotInitializedException.java" +) + +### Macro ### macro(target) if(NOT "${target_suffix} " STREQUAL " ") get_rcl_information("${rmw_implementation}" "rcl${target_suffix}") endif() - set(_jni_classes "RCLJava;Node;Publisher;Client") + set(_jni_classes "RCLJava;node_Node;node_topic_Publisher;node_service_Client") foreach(_jni_class ${_jni_classes}) - set(_target_name "${PROJECT_NAME}${_jni_class}${target_suffix}") add_library( ${_target_name} - SHARED src/main/cpp/org_ros2_rcljava_${_jni_class}.cpp + SHARED + src/main/cpp/org_ros2_rcljava_${_jni_class}.cpp + ) + + target_compile_definitions( + ${_target_name} + PRIVATE + "RMW_IMPLEMENTATION_SUFFIX=${target_suffix}" ) - target_compile_definitions(${_target_name} - PRIVATE "RMW_IMPLEMENTATION_SUFFIX=${target_suffix}") if(WIN32) set(_build_types ";_DEBUG;_MINSIZEREL;_RELEASE;_RELWITHDEBINFO") @@ -54,23 +138,29 @@ macro(target) endif() foreach(_build_type ${_build_types}) - set_target_properties(${_target_name} PROPERTIES - LIBRARY_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}" - RUNTIME_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}" + set_target_properties( + ${_target_name} + PROPERTIES + LIBRARY_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}" + RUNTIME_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}" ) endforeach() ament_target_dependencies(${_target_name} "rcl${target_suffix}" "rcljava_common" + "rcl_interfaces" ) - target_include_directories(${_target_name} + target_include_directories( + ${_target_name} PUBLIC - ${JNI_INCLUDE_DIRS} + ${JNI_INCLUDE_DIRS} ) - ament_export_libraries(${_target_name}) + if(NOT rosidl_generate_interfaces_SKIP_INSTALL) + ament_export_libraries(${_target_name}) + endif() install(TARGETS ${_target_name} ARCHIVE DESTINATION lib @@ -81,32 +171,17 @@ macro(target) endmacro() -set(${PROJECT_NAME}_sources - "src/main/java/org/ros2/rcljava/BiConsumer.java" - "src/main/java/org/ros2/rcljava/Client.java" - "src/main/java/org/ros2/rcljava/Consumer.java" - "src/main/java/org/ros2/rcljava/Node.java" - "src/main/java/org/ros2/rcljava/package-info.java" - "src/main/java/org/ros2/rcljava/Publisher.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/QoSPolicy.java" - "src/main/java/org/ros2/rcljava/qos/policies/Reliability.java" - "src/main/java/org/ros2/rcljava/qos/QoSProfile.java" - "src/main/java/org/ros2/rcljava/RCLFuture.java" - "src/main/java/org/ros2/rcljava/RCLJava.java" - "src/main/java/org/ros2/rcljava/RMWRequestId.java" - "src/main/java/org/ros2/rcljava/Service.java" - "src/main/java/org/ros2/rcljava/Subscription.java" - "src/main/java/org/ros2/rcljava/TriConsumer.java" -) - +set(CMAKE_JAVA_INCLUDE_PATH "${BUILTIN_COMMON_JARS}:${BUILTIN_MSGS_JARS}:${LOG_JAR}") add_jar("${PROJECT_NAME}_jar" "${${PROJECT_NAME}_sources}" OUTPUT_NAME "${PROJECT_NAME}" INCLUDE_JARS + "${std_msgs_JARS}" + "${rcl_interfaces_JARS}" + "${builtin_interfaces_JARS}" "${rcljava_common_JARS}" + "${_jar_file}" ) call_for_each_rmw_implementation(target) @@ -148,16 +223,22 @@ if(BUILD_TESTING) PROPERTY "JAR_FILE") set(${PROJECT_NAME}_test_sources + "src/test/java/org/ros2/rcljava/GraphNameTest.java" "src/test/java/org/ros2/rcljava/NodeTest.java" + "src/test/java/org/ros2/rcljava/MessageTest.java" "src/test/java/org/ros2/rcljava/PublisherTest.java" "src/test/java/org/ros2/rcljava/RCLJavaTest.java" + "src/test/java/org/ros2/rcljava/RmwTest.java" "src/test/java/org/ros2/rcljava/SubscriptionTest.java" ) set(${PROJECT_NAME}_testsuites + "org.ros2.rcljava.GraphNameTest" "org.ros2.rcljava.NodeTest" + "org.ros2.rcljava.MessageTest" "org.ros2.rcljava.PublisherTest" "org.ros2.rcljava.RCLJavaTest" + "org.ros2.rcljava.RmwTest" "org.ros2.rcljava.SubscriptionTest" ) @@ -170,11 +251,18 @@ if(BUILD_TESTING) get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) list(APPEND _deps_library_paths ${_dep_dir}) endforeach() + foreach(_dep_lib ${rcl_interfaces_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list(APPEND _deps_library_paths ${_dep_dir}) + endforeach() foreach(_dep_lib ${builtin_interfaces_LIBRARIES}) get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) list(APPEND _deps_library_paths ${_dep_dir}) endforeach() list(APPEND _deps_library_paths ${CMAKE_CURRENT_BINARY_DIR}) + + list(REMOVE_DUPLICATES _deps_library_paths) + list(SORT _deps_library_paths) list(APPEND _deps_library_paths ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_java/rcljava/msg/) foreach(testsuite ${${PROJECT_NAME}_testsuites}) @@ -187,8 +275,10 @@ if(BUILD_TESTING) "${rcljava_test_msgs_JARS}" "${std_msgs_JARS}" "${builtin_interfaces_JARS}" + "${rcl_interfaces_JARS}" "${_${PROJECT_NAME}_jar_file}" "${_${PROJECT_NAME}_messages_jar_file}" + "${_jar_file}" LIBRARY_PATHS "${_deps_library_paths}" ) diff --git a/rcljava/include/org_ros2_rcljava_Node.h b/rcljava/include/org_ros2_rcljava_Node.h deleted file mode 100644 index ce5a1796..00000000 --- a/rcljava/include/org_ros2_rcljava_Node.h +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2016 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_Node */ - -#ifndef ORG_ROS2_RCLJAVA_NODE_H_ -#define ORG_ROS2_RCLJAVA_NODE_H_ - -#ifdef __cplusplus -extern "C" { -#endif -/* - * Class: org_ros2_rcljava_Node - * Method: nativeCreatePublisherHandle - * Signature: (JLjava/lang/Class;Ljava/lang/String;J)J - */ -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreatePublisherHandle - (JNIEnv *, jclass, jlong, jclass, jstring, jlong); - -/* - * Class: org_ros2_rcljava_Node - * Method: nativeCreateSubscriptionHandle - * Signature: (JLjava/lang/Class;Ljava/lang/String;J)J - */ -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreateSubscriptionHandle - (JNIEnv *, jclass, jlong, jclass, jstring, jlong); - -/* - * Class: org_ros2_rcljava_Node - * Method: nativeCreateServiceHandle - * Signature: (JLjava/lang/Class;Ljava/lang/String;J)J - */ -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreateServiceHandle - (JNIEnv *, jclass, jlong, jclass, jstring, jlong); - -/* - * Class: org_ros2_rcljava_Node - * Method: nativeCreateClientHandle - * Signature: (JLjava/lang/Class;Ljava/lang/String;J)J - */ -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreateClientHandle - (JNIEnv *, jclass, jlong, jclass, jstring, jlong); - -#ifdef __cplusplus -} -#endif - -#endif // ORG_ROS2_RCLJAVA_NODE_H_ diff --git a/rcljava/include/org_ros2_rcljava_RCLJava.h b/rcljava/include/rcljava/org_ros2_rcljava_RCLJava.h similarity index 87% rename from rcljava/include/org_ros2_rcljava_RCLJava.h rename to rcljava/include/rcljava/org_ros2_rcljava_RCLJava.h index 4fc54bff..3dcf1c22 100644 --- a/rcljava/include/org_ros2_rcljava_RCLJava.h +++ b/rcljava/include/rcljava/org_ros2_rcljava_RCLJava.h @@ -1,4 +1,5 @@ // Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,22 +13,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +/* DO NOT EDIT THIS FILE - it is machine generated */ #include /* Header for class org_ros2_rcljava_RCLJava */ -#ifndef ORG_ROS2_RCLJAVA_RCLJAVA_H_ -#define ORG_ROS2_RCLJAVA_RCLJAVA_H_ - +#ifndef RCLJAVA__ORG_ROS2_RCLJAVA_RCLJAVA_H_ +#define RCLJAVA__ORG_ROS2_RCLJAVA_RCLJAVA_H_ #ifdef __cplusplus extern "C" { #endif + /* * Class: org_ros2_rcljava_RCLJava * Method: nativeRCLJavaInit - * Signature: ()V + * Signature: ([Ljava/lang/String;)V */ JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeRCLJavaInit - (JNIEnv *, jclass); + (JNIEnv *, jclass, jobjectArray); /* * Class: org_ros2_rcljava_RCLJava @@ -53,14 +55,6 @@ JNIEXPORT jstring JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetRMWIdentifier JNIEXPORT jboolean JNICALL Java_org_ros2_rcljava_RCLJava_nativeOk (JNIEnv *, jclass); -/* - * Class: org_ros2_rcljava_RCLJava - * Method: nativeShutdown - * Signature: ()V - */ -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeShutdown - (JNIEnv *, jclass); - /* * Class: org_ros2_rcljava_RCLJava * Method: nativeGetZeroInitializedWaitSet @@ -109,6 +103,31 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWait JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTake (JNIEnv *, jclass, jlong, jclass); +/* + * Class: org_ros2_rcljava_RCLJava + * Method: nativeShutdown + * Signature: ()V + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeShutdown + (JNIEnv *, jclass); + +/* + * Class: org_ros2_rcljava_RCLJava + * Method: nativeWaitSetFini + * Signature: (J)V + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetFini + (JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_RCLJava + * Method: nativeGetNodeNames + * Signature: ()Ljava/util/List; + */ +JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetNodeNames + (JNIEnv *, jclass); + + /* * Class: org_ros2_rcljava_RCLJava * Method: nativeWaitSetClearServices @@ -184,5 +203,4 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeDisposeQoSProfile #ifdef __cplusplus } #endif - -#endif // ORG_ROS2_RCLJAVA_RCLJAVA_H_ +#endif // RCLJAVA__ORG_ROS2_RCLJAVA_RCLJAVA_H_ diff --git a/rcljava/include/rcljava/org_ros2_rcljava_node_Node.h b/rcljava/include/rcljava/org_ros2_rcljava_node_Node.h new file mode 100644 index 00000000..3c194309 --- /dev/null +++ b/rcljava/include/rcljava/org_ros2_rcljava_node_Node.h @@ -0,0 +1,100 @@ +// Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard +// +// 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. + +/* DO NOT EDIT THIS FILE - it is machine generated */ +#include +/* Header for class org_ros2_rcljava_node_Node */ + +#ifndef RCLJAVA__ORG_ROS2_RCLJAVA_NODE_NODE_H_ +#define RCLJAVA__ORG_ROS2_RCLJAVA_NODE_NODE_H_ +#ifdef __cplusplus +extern "C" { +#endif +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeCreatePublisherHandle + * Signature: (JLjava/lang/Class;Ljava/lang/String;J)J + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreatePublisherHandle + (JNIEnv *, jclass, jlong, jclass, jstring, jlong); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeCreateSubscriptionHandle + * Signature: (JLjava/lang/Class;Ljava/lang/String;J)J + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreateSubscriptionHandle + (JNIEnv *, jclass, jlong, jclass, jstring, jlong); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeCreateClientHandle + * Signature: (JLjava/lang/Class;Ljava/lang/String;J)J + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreateClientHandle + (JNIEnv *, jclass, jlong, jclass, jstring, jlong); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeCreateServiceHandle + * Signature: (JLjava/lang/Class;Ljava/lang/String;J;)J + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreateServiceHandle + (JNIEnv *, jclass, jlong, jclass, jstring, jlong); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeDispose + * Signature: (J)V + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_Node_nativeDispose + (JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeGetName + * Signature: (J)Ljava/lang/String; + */ +JNIEXPORT jstring JNICALL Java_org_ros2_rcljava_node_Node_nativeGetName + (JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeCountPublishers + * Signature: (JLjava/lang/String;)I + */ +JNIEXPORT jint JNICALL Java_org_ros2_rcljava_node_Node_nativeCountPublishers + (JNIEnv *, jclass, jlong, jstring); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: nativeCountSubscribers + * Signature: (JLjava/lang/String;)I + */ +JNIEXPORT jint JNICALL Java_org_ros2_rcljava_node_Node_nativeCountSubscribers + (JNIEnv *, jclass, jlong, jstring); + +/* + * Class: org_ros2_rcljava_node_Node + * Method: getListTopics + * Signature: (J)Ljava/util/HashMap; + */ +JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_node_Node_getListTopics + (JNIEnv *, jclass, jlong); + +#ifdef __cplusplus +} +#endif +#endif // RCLJAVA__ORG_ROS2_RCLJAVA_NODE_NODE_H_ diff --git a/rcljava/include/org_ros2_rcljava_Client.h b/rcljava/include/rcljava/org_ros2_rcljava_node_service_Client.h similarity index 67% rename from rcljava/include/org_ros2_rcljava_Client.h rename to rcljava/include/rcljava/org_ros2_rcljava_node_service_Client.h index b23c338e..b7bbd2a9 100644 --- a/rcljava/include/org_ros2_rcljava_Client.h +++ b/rcljava/include/rcljava/org_ros2_rcljava_node_service_Client.h @@ -1,4 +1,5 @@ // Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,24 +13,25 @@ // See the License for the specific language governing permissions and // limitations under the License. +/* DO NOT EDIT THIS FILE - it is machine generated */ #include /* Header for class org_ros2_rcljava_Client */ -#ifndef ORG_ROS2_RCLJAVA_CLIENT_H_ -#define ORG_ROS2_RCLJAVA_CLIENT_H_ +#ifndef RCLJAVA__ORG_ROS2_RCLJAVA_NODE_SERVICE_CLIENT_H_ +#define RCLJAVA__ORG_ROS2_RCLJAVA_NODE_SERVICE_CLIENT_H_ #ifdef __cplusplus extern "C" { #endif /* - * Class: org_ros2_rcljava_Client + * Class: org_ros2_rcljava_node_service_Client * Method: nativeSendClientRequest * Signature: (JJJJLjava/lang/Object;)V */ -JNIEXPORT void JNICALL Java_org_ros2_rcljava_Client_nativeSendClientRequest +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_service_Client_nativeSendClientRequest (JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); #ifdef __cplusplus } #endif -#endif // ORG_ROS2_RCLJAVA_CLIENT_H_ +#endif // RCLJAVA__ORG_ROS2_RCLJAVA_NODE_SERVICE_CLIENT_H_ diff --git a/rcljava/include/org_ros2_rcljava_Publisher.h b/rcljava/include/rcljava/org_ros2_rcljava_node_topic_Publisher.h similarity index 57% rename from rcljava/include/org_ros2_rcljava_Publisher.h rename to rcljava/include/rcljava/org_ros2_rcljava_node_topic_Publisher.h index 82e1e9ee..86c2544b 100644 --- a/rcljava/include/org_ros2_rcljava_Publisher.h +++ b/rcljava/include/rcljava/org_ros2_rcljava_node_topic_Publisher.h @@ -1,4 +1,5 @@ // Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,33 +13,33 @@ // See the License for the specific language governing permissions and // limitations under the License. +/* DO NOT EDIT THIS FILE - it is machine generated */ #include -/* Header for class org_ros2_rcljava_Publisher */ - -#ifndef ORG_ROS2_RCLJAVA_PUBLISHER_H_ -#define ORG_ROS2_RCLJAVA_PUBLISHER_H_ +/* Header for class org_ros2_rcljava_node_topic_Publisher */ +#ifndef RCLJAVA__ORG_ROS2_RCLJAVA_NODE_TOPIC_PUBLISHER_H_ +#define RCLJAVA__ORG_ROS2_RCLJAVA_NODE_TOPIC_PUBLISHER_H_ #ifdef __cplusplus extern "C" { #endif /* - * Class: org_ros2_rcljava_Publisher + * Class: org_ros2_rcljava_node_topic_Publisher * Method: nativePublish - * Signature: (JLjava/lang/Object;) + * Signature: (JLjava/lang/Object)V */ -JNIEXPORT void JNICALL Java_org_ros2_rcljava_Publisher_nativePublish +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_topic_Publisher_nativePublish (JNIEnv *, jclass, jlong, jobject); /* - * Class: org_ros2_rcljava_Node + * Class: org_ros2_rcljava_node_topic_Publisher * Method: nativeDispose * Signature: (JJ)V */ -JNIEXPORT void JNICALL Java_org_ros2_rcljava_Publisher_nativeDispose +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_topic_Publisher_nativeDispose (JNIEnv *, jclass, jlong, jlong); #ifdef __cplusplus } #endif -#endif // ORG_ROS2_RCLJAVA_PUBLISHER_H_ +#endif // RCLJAVA__ORG_ROS2_RCLJAVA_NODE_TOPIC_PUBLISHER_H_ diff --git a/rcljava/include/rcljava/utils.h b/rcljava/include/rcljava/utils.h new file mode 100644 index 00000000..6e592f59 --- /dev/null +++ b/rcljava/include/rcljava/utils.h @@ -0,0 +1,294 @@ +// Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard +// +// 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 + +#include "rcl/graph.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rosidl_generator_c/message_type_support.h" + +#ifndef RCLJAVA__UTILS_H_ +#define RCLJAVA__UTILS_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Convert jstring to std::string. + */ +std::string +jstring2String(JNIEnv * env, jstring jsubject) +{ + const char * subject_tmp = env->GetStringUTFChars(jsubject, 0); + std::string result(subject_tmp); + env->ReleaseStringUTFChars(jsubject, subject_tmp); + + return result; +} + +/* + * Convertion of Message type. + */ +rosidl_message_type_support_t * +jclass2MessageType(JNIEnv * env, jclass jmessage_class) +{ + jmethodID mid = + env->GetStaticMethodID(jmessage_class, "getTypeSupport", "()J"); + + jlong jts = env->CallStaticLongMethod(jmessage_class, mid); + + rosidl_message_type_support_t * ts = + reinterpret_cast(jts); + + return ts; +} + +/* + * + */ +void * +jclass2Message(JNIEnv * env, jclass jmessage_class) +{ + jmethodID jfrom_mid = env->GetStaticMethodID(jmessage_class, "getFromJavaConverter", "()J"); + jlong jfrom_java_converter = env->CallStaticLongMethod(jmessage_class, jfrom_mid); + + using convert_from_java_signature = void * (*)(jobject, void *); + convert_from_java_signature convert_from_java = + reinterpret_cast(jfrom_java_converter); + + jmethodID jconstructor = env->GetMethodID(jmessage_class, "", "()V"); + jobject jmsg = env->NewObject(jmessage_class, jconstructor); + + void * taken_msg = convert_from_java(jmsg, nullptr); + return taken_msg; +} + +jobject +jclass2JMessage(JNIEnv * env, jclass jmessage_class, void * taken_msg) +{ + jmethodID jto_mid = env->GetStaticMethodID(jmessage_class, "getToJavaConverter", "()J"); + jlong jto_java_converter = env->CallStaticLongMethod(jmessage_class, jto_mid); + + using convert_to_java_signature = jobject (*)(void *, jobject); + convert_to_java_signature convert_to_java = + reinterpret_cast(jto_java_converter); + + jobject jtaken_msg = convert_to_java(taken_msg, nullptr); + return jtaken_msg; +} + +/* + * + */ +void * +jobject2Message(JNIEnv * env, jobject jmessage) +{ + jclass jmessage_class = env->GetObjectClass(jmessage); + jmethodID mid = env->GetStaticMethodID(jmessage_class, "getFromJavaConverter", "()J"); + jlong jfrom_java_converter = env->CallStaticLongMethod(jmessage_class, mid); + + using convert_from_java_signature = void * (*)(jobject, void *); + convert_from_java_signature convert_from_java = + reinterpret_cast(jfrom_java_converter); + + void * raw_ros_message = convert_from_java(jmessage, nullptr); + return raw_ros_message; +} + +/* + * + */ +rosidl_service_type_support_t * +jclass2ServiceType(JNIEnv * env, jclass jmessage_class) +{ + jmethodID mid = + env->GetStaticMethodID(jmessage_class, "getServiceTypeSupport", "()J"); + assert(mid != NULL); + + jlong jts = env->CallStaticLongMethod(jmessage_class, mid); + assert(jts != 0); + + rosidl_service_type_support_t * ts = + reinterpret_cast(jts); + + return ts; +} + +/* + * + */ +jlong +instance2Handle(void * obj) +{ + jlong handler = reinterpret_cast(obj); + + assert(handler != 0); + + return handler; +} + +/* + * + */ +void +throwException(JNIEnv * env, std::string message) +{ + const char * class_name = "java/lang/IllegalStateException"; + jclass exception_class = env->FindClass(class_name); + + assert(exception_class != NULL); + + env->ThrowNew(exception_class, message.c_str()); +} + +/* + * + */ +jobject +makeJTopics(JNIEnv * env, rcl_topic_names_and_types_t * topic_names_and_types) +{ + env->PushLocalFrame(256); // fix for local references + + jsize map_len = topic_names_and_types->topic_count; + jclass mapClass = env->FindClass("java/util/HashMap"); + jmethodID init = env->GetMethodID(mapClass, "", "(I)V"); + jobject hashMap = env->NewObject(mapClass, init, map_len); + jmethodID put = env->GetMethodID(mapClass, "put", + "(Ljava/lang/Object;Ljava/lang/Object;)Ljava/lang/Object;"); + + for (size_t i = 0; i < topic_names_and_types->topic_count; ++i) { + env->CallObjectMethod(hashMap, put, + env->NewStringUTF(topic_names_and_types->topic_names[i]), + env->NewStringUTF(topic_names_and_types->type_names[i])); + } + + env->PopLocalFrame(hashMap); + + return hashMap; +} + +/* +jobject +makeJNames(rmw_ros_meta_t * ros_meta) +{ + java_util_ArrayList = static_cast(env->NewGlobalRef(env->FindClass("java/util/ArrayList"))); + java_util_ArrayList_ = env->GetMethodID(java_util_ArrayList, "", "(I)V"); + java_util_ArrayList_size = env->GetMethodID (java_util_ArrayList, "size", "()I"); +// java_util_ArrayList_get = env->GetMethodID(java_util_ArrayList, "get", "(I)Ljava/lang/Object;"); + java_util_ArrayList_add = env->GetMethodID(java_util_ArrayList, "add", "(Ljava/lang/Object;)Z"); + + jobject result = env->NewObject(java_util_ArrayList, java_util_ArrayList_, vector.size()); + + for (size_t i = 0; i < rmw_ros_meta_t->topic_count; ++i) { + jstring element = env->NewStringUTF(ros_meta_data->node_names[i].c_str()); + + env->CallBooleanMethod(result, java_util_ArrayList_add, element); + env->DeleteLocalRef(element); + } + return result; +} +*/ + +jobject convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) +{ + jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/node/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; +} + +#ifdef __cplusplus +} +#endif + +/* + * + */ +template +T * +makeInstance() +{ + return (T *)malloc(sizeof(T)); +} + +/* + * + */ +template +T * +handle2Instance(jlong handle) +{ + T * obj = reinterpret_cast(handle); + + assert(obj != NULL); + + return obj; +} + +#endif // RCLJAVA__UTILS_H_ diff --git a/rcljava/package.xml b/rcljava/package.xml index aa334111..b135156d 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -12,16 +12,21 @@ rosidl_cmake - rmw_implementation - rmw_implementation_cmake - rosidl_generator_c - rosidl_parser - rmw_implementation_cmake + rosidl_generator_java + rcljava_common rcl rmw + std_msgs + rmw_implementation + rmw_implementation_cmake + rosidl_generator_c + rosidl_generator_java + rosidl_parser + rcljava_common rcl + std_msgs ament_lint_auto ament_lint_common diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_Node.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_Node.cpp deleted file mode 100644 index b57d3fa4..00000000 --- a/rcljava/src/main/cpp/org_ros2_rcljava_Node.cpp +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright 2016 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 -#include - -#include "rmw/rmw.h" -#include "rcl/error_handling.h" -#include "rcl/rcl.h" -#include "rcl/node.h" -#include "rosidl_generator_c/message_type_support_struct.h" - -#include "rcljava_common/exceptions.h" -#include "rcljava_common/signatures.h" - -#include "org_ros2_rcljava_Node.h" - -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreatePublisherHandle(JNIEnv * env, jclass, - jlong node_handle, - jclass jmessage_class, - jstring jtopic, - jlong qos_profile_handle) -{ - jmethodID mid = env->GetStaticMethodID(jmessage_class, "getTypeSupport", "()J"); - jlong jts = env->CallStaticLongMethod(jmessage_class, mid); - - const char * topic_tmp = env->GetStringUTFChars(jtopic, 0); - - std::string topic(topic_tmp); - - env->ReleaseStringUTFChars(jtopic, topic_tmp); - - rcl_node_t * node = reinterpret_cast(node_handle); - - rosidl_message_type_support_t * ts = - reinterpret_cast(jts); - - rcl_publisher_t * publisher = static_cast(malloc(sizeof(rcl_publisher_t))); - publisher->impl = NULL; - rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); - - rmw_qos_profile_t * qos_profile = reinterpret_cast(qos_profile_handle); - publisher_ops.qos = *qos_profile; - - rcl_ret_t ret = rcl_publisher_init(publisher, node, ts, topic.c_str(), &publisher_ops); - - if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to create publisher: " + std::string(rcl_get_error_string_safe())); - return 0; - } - - jlong jpublisher = reinterpret_cast(publisher); - return jpublisher; -} - -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreateSubscriptionHandle(JNIEnv * env, - jclass, - jlong node_handle, - jclass jmessage_class, - jstring jtopic, - jlong qos_profile_handle) -{ - jmethodID mid = env->GetStaticMethodID(jmessage_class, "getTypeSupport", "()J"); - jlong jts = env->CallStaticLongMethod(jmessage_class, mid); - - const char * topic_tmp = env->GetStringUTFChars(jtopic, 0); - - std::string topic(topic_tmp); - - env->ReleaseStringUTFChars(jtopic, topic_tmp); - - rcl_node_t * node = reinterpret_cast(node_handle); - - rosidl_message_type_support_t * ts = - reinterpret_cast(jts); - - rcl_subscription_t * subscription = - static_cast(malloc(sizeof(rcl_subscription_t))); - subscription->impl = NULL; - rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); - - rmw_qos_profile_t * qos_profile = reinterpret_cast(qos_profile_handle); - subscription_ops.qos = *qos_profile; - - rcl_ret_t ret = rcl_subscription_init(subscription, node, ts, topic.c_str(), &subscription_ops); - - if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to create subscription: " + std::string(rcl_get_error_string_safe())); - return 0; - } - - jlong jsubscription = reinterpret_cast(subscription); - return jsubscription; -} - -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreateServiceHandle(JNIEnv * env, jclass, - jlong node_handle, jclass jservice_class, jstring jservice_name, jlong qos_profile_handle) -{ - jmethodID mid = env->GetStaticMethodID(jservice_class, "getServiceTypeSupport", "()J"); - - assert(mid != NULL); - - jlong jts = env->CallStaticLongMethod(jservice_class, mid); - - assert(jts != 0); - - const char * service_name_tmp = env->GetStringUTFChars(jservice_name, 0); - - std::string service_name(service_name_tmp); - - env->ReleaseStringUTFChars(jservice_name, service_name_tmp); - - rcl_node_t * node = reinterpret_cast(node_handle); - - rosidl_service_type_support_t * ts = - reinterpret_cast(jts); - - rcl_service_t * service = static_cast(malloc(sizeof(rcl_service_t))); - service->impl = NULL; - rcl_service_options_t service_ops = rcl_service_get_default_options(); - - rmw_qos_profile_t * qos_profile = reinterpret_cast(qos_profile_handle); - service_ops.qos = *qos_profile; - - rcl_ret_t ret = rcl_service_init(service, node, ts, service_name.c_str(), &service_ops); - - if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to create service: " + std::string(rcl_get_error_string_safe())); - return 0; - } - - jlong jservice = reinterpret_cast(service); - return jservice; -} - -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_Node_nativeCreateClientHandle(JNIEnv * env, jclass, - jlong node_handle, jclass jservice_class, jstring jservice_name, jlong qos_profile_handle) -{ - jmethodID mid = env->GetStaticMethodID(jservice_class, "getServiceTypeSupport", "()J"); - - assert(mid != NULL); - - jlong jts = env->CallStaticLongMethod(jservice_class, mid); - - assert(jts != 0); - - const char * service_name_tmp = env->GetStringUTFChars(jservice_name, 0); - - std::string service_name(service_name_tmp); - - env->ReleaseStringUTFChars(jservice_name, service_name_tmp); - - rcl_node_t * node = reinterpret_cast(node_handle); - - rosidl_service_type_support_t * ts = - reinterpret_cast(jts); - - rcl_client_t * client = static_cast(malloc(sizeof(rcl_client_t))); - client->impl = NULL; - rcl_client_options_t client_ops = rcl_client_get_default_options(); - - rmw_qos_profile_t * qos_profile = reinterpret_cast(qos_profile_handle); - client_ops.qos = *qos_profile; - - rcl_ret_t ret = rcl_client_init(client, node, ts, service_name.c_str(), &client_ops); - - if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to create client: " + std::string(rcl_get_error_string_safe())); - return 0; - } - - jlong jclient = reinterpret_cast(client); - return jclient; -} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_Publisher.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_Publisher.cpp deleted file mode 100644 index 4f6687d5..00000000 --- a/rcljava/src/main/cpp/org_ros2_rcljava_Publisher.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2016 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 -#include - -#include "rmw/rmw.h" -#include "rcl/error_handling.h" -#include "rcl/rcl.h" -#include "rcl/node.h" - -#include "rcljava_common/exceptions.h" -#include "rcljava_common/signatures.h" - -#include "org_ros2_rcljava_Publisher.h" - -JNIEXPORT void JNICALL Java_org_ros2_rcljava_Publisher_nativePublish(JNIEnv * env, jclass, - jlong publisher_handle, - jobject jmsg) -{ - rcl_publisher_t * publisher = reinterpret_cast(publisher_handle); - - jclass jmessage_class = env->GetObjectClass(jmsg); - - jmethodID mid = env->GetStaticMethodID(jmessage_class, "getFromJavaConverter", "()J"); - jlong jfrom_java_converter = env->CallStaticLongMethod(jmessage_class, mid); - - convert_from_java_signature convert_from_java = - reinterpret_cast(jfrom_java_converter); - - void * raw_ros_message = convert_from_java(jmsg, nullptr); - - rcl_ret_t ret = rcl_publish(publisher, raw_ros_message); - if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to publish: " + std::string(rcl_get_error_string_safe())); - } -} - -JNIEXPORT void JNICALL Java_org_ros2_rcljava_Publisher_nativeDispose(JNIEnv * env, jclass, - jlong node_handle, - jlong publisher_handle) -{ - rcl_node_t * node = reinterpret_cast(node_handle); - - assert(node != NULL); - - rcl_publisher_t * publisher = reinterpret_cast(publisher_handle); - - assert(publisher != NULL); - - rcl_ret_t ret = rcl_publisher_fini(publisher, node); - - if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to destroy publisher: " + std::string(rcl_get_error_string_safe())); - } -} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_RCLJava.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_RCLJava.cpp index 23e4e86c..8219cb50 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_RCLJava.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_RCLJava.cpp @@ -1,4 +1,5 @@ // Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,67 +23,115 @@ #include "rcl/error_handling.h" #include "rcl/rcl.h" #include "rcl/node.h" + #include "rosidl_generator_c/message_type_support.h" #include "rcljava_common/exceptions.h" #include "rcljava_common/signatures.h" -#include "org_ros2_rcljava_RCLJava.h" +#include "rcljava/org_ros2_rcljava_RCLJava.h" +#include "rcljava/utils.h" jobject convert_rmw_request_id_to_java(JNIEnv *, rmw_request_id_t *); rmw_request_id_t * convert_rmw_request_id_from_java(JNIEnv *, jobject); -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeRCLJavaInit(JNIEnv * env, jclass) +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeRCLJavaInit( + JNIEnv * env, + jclass, + jobjectArray arg) { // TODO(esteve): parse args - rcl_ret_t ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + int argc = arg != NULL ? env->GetArrayLength(arg) : 0; + char ** argv = nullptr; + rcl_allocator_t allocator = rcl_get_default_allocator(); + + rcl_ret_t ret = rcl_init(argc, argv, allocator); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to init: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to init: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_RCLJava_nativeCreateNodeHandle(JNIEnv * env, jclass, - jstring jnode_name) +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeShutdown( + JNIEnv * env, + jclass) { - const char * node_name_tmp = env->GetStringUTFChars(jnode_name, 0); - - std::string node_name(node_name_tmp); - - env->ReleaseStringUTFChars(jnode_name, node_name_tmp); - - rcl_node_t * node = static_cast(malloc(sizeof(rcl_node_t))); - node->impl = nullptr; - rcl_node_options_t default_options = rcl_node_get_default_options(); - rcl_ret_t ret = rcl_node_init(node, node_name.c_str(), &default_options); + rcl_ret_t ret = rcl_shutdown(); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to create node: " + std::string(rcl_get_error_string_safe())); - return 0; + std::string message("Failed to shutdown: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } - jlong node_handle = reinterpret_cast(node); - return node_handle; } -JNIEXPORT jstring JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetRMWIdentifier(JNIEnv * env, jclass) +/* + * + */ +JNIEXPORT jboolean JNICALL Java_org_ros2_rcljava_RCLJava_nativeOk( + JNIEnv *, + jclass) +{ + return rcl_ok(); +} + +/* + * + */ +JNIEXPORT jstring JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetRMWIdentifier( + JNIEnv * env, + jclass) { const char * rmw_implementation_identifier = rmw_get_implementation_identifier(); return env->NewStringUTF(rmw_implementation_identifier); } -JNIEXPORT jboolean JNICALL Java_org_ros2_rcljava_RCLJava_nativeOk(JNIEnv *, jclass) +/* + * + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_RCLJava_nativeCreateNodeHandle( + JNIEnv * env, + jclass, + jstring jnode_name) { - return rcl_ok(); + std::string node_name = jstring2String(env, jnode_name); + + rcl_node_t * node = makeInstance(); + node->impl = nullptr; + + rcl_node_options_t default_options = rcl_node_get_default_options(); + + rcl_ret_t ret = rcl_node_init(node, node_name.c_str(), &default_options); + if (ret != RCL_RET_OK) { + std::string message("Failed to create node: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + + return 0; + } + + jlong node_handle = instance2Handle(node); + return node_handle; } -JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetZeroInitializedWaitSet(JNIEnv *, + +/* + * + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetZeroInitializedWaitSet( + JNIEnv *, jclass) { - rcl_wait_set_t * wait_set = static_cast(malloc(sizeof(rcl_wait_set_t))); + // ~ rcl_get_zero_initialized_wait_set(); + rcl_wait_set_t * wait_set = makeInstance(); wait_set->subscriptions = nullptr; wait_set->size_of_subscriptions = 0; wait_set->guard_conditions = nullptr; @@ -94,102 +143,123 @@ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetZeroInitializedWa wait_set->services = nullptr; wait_set->size_of_services = 0; wait_set->impl = nullptr; - jlong wait_set_handle = reinterpret_cast(wait_set); + + jlong wait_set_handle = instance2Handle(wait_set); return wait_set_handle; } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetInit(JNIEnv * env, jclass, +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetInit( + JNIEnv * env, + jclass, jlong wait_set_handle, jint number_of_subscriptions, - jint number_of_guard_conditions, jint number_of_timers, - jint number_of_clients, jint number_of_services) + jint number_of_guard_conditions, + jint number_of_timers, + jint number_of_clients, + jint number_of_services +) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); rcl_ret_t ret = rcl_wait_set_init( - wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, - number_of_clients, number_of_services, rcl_get_default_allocator()); + wait_set, + number_of_subscriptions, + number_of_guard_conditions, + number_of_timers, + number_of_clients, + number_of_services, + rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to initialize wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to initialize wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetClearSubscriptions(JNIEnv * env, +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetClearSubscriptions( + JNIEnv * env, jclass, jlong wait_set_handle) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + rcl_ret_t ret = rcl_wait_set_clear_subscriptions(wait_set); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to clear subscriptions from wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to clear subscriptions from wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetAddSubscription(JNIEnv * env, +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetAddSubscription( + JNIEnv * env, jclass, jlong wait_set_handle, jlong subscription_handle) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); - rcl_subscription_t * subscription = reinterpret_cast(subscription_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + rcl_subscription_t * subscription = + handle2Instance(subscription_handle); + rcl_ret_t ret = rcl_wait_set_add_subscription(wait_set, subscription); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to add subscription to wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to add subscription to wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWait(JNIEnv * env, jclass, +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWait( + JNIEnv * env, + jclass, jlong wait_set_handle) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + rcl_ret_t ret = rcl_wait(wait_set, RCL_S_TO_NS(1)); if (ret != RCL_RET_OK && ret != RCL_RET_TIMEOUT) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to wait on wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to wait on wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTake(JNIEnv * env, jclass, +/* + * + */ +JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTake( + JNIEnv * env, + jclass, jlong subscription_handle, jclass jmessage_class) { - rcl_subscription_t * subscription = reinterpret_cast(subscription_handle); - - jmethodID jfrom_mid = env->GetStaticMethodID(jmessage_class, "getFromJavaConverter", "()J"); - jlong jfrom_java_converter = env->CallStaticLongMethod(jmessage_class, jfrom_mid); + rcl_subscription_t * subscription = + handle2Instance(subscription_handle); - convert_from_java_signature convert_from_java = - reinterpret_cast(jfrom_java_converter); - - jmethodID jconstructor = env->GetMethodID(jmessage_class, "", "()V"); - jobject jmsg = env->NewObject(jmessage_class, jconstructor); - - void * taken_msg = convert_from_java(jmsg, nullptr); + void * taken_msg = jclass2Message(env, jmessage_class); rcl_ret_t ret = rcl_take(subscription, taken_msg, nullptr); - if (ret != RCL_RET_OK && ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to take from a subscription: " + std::string(rcl_get_error_string_safe())); - return nullptr; + std::string message("Failed to take from a subscription: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - jmethodID jto_mid = env->GetStaticMethodID(jmessage_class, "getToJavaConverter", "()J"); - jlong jto_java_converter = env->CallStaticLongMethod(jmessage_class, jto_mid); - - convert_to_java_signature convert_to_java = - reinterpret_cast(jto_java_converter); - - jobject jtaken_msg = convert_to_java(taken_msg, nullptr); + jobject jtaken_msg = jclass2JMessage(env, jmessage_class, taken_msg); return jtaken_msg; } @@ -197,81 +267,111 @@ JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTake(JNIEnv * env, return nullptr; } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeShutdown(JNIEnv * env, jclass) +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetFini( + JNIEnv * env, + jclass, + jlong wait_set_handle) { - rcl_ret_t ret = rcl_shutdown(); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + + rcl_ret_t ret = rcl_wait_set_fini(wait_set); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to shutdown: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to release wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetClearServices(JNIEnv * env, + +JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeGetNodeNames( + JNIEnv *, + jclass) +{ + return NULL; +} + + +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetClearServices( + JNIEnv * env, jclass, jlong wait_set_handle) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + rcl_ret_t ret = rcl_wait_set_clear_services(wait_set); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to clear services from wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to clear services from wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetAddService(JNIEnv * env, jclass, +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetAddService( + JNIEnv * env, + jclass, jlong wait_set_handle, jlong service_handle) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); - rcl_service_t * service = reinterpret_cast(service_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + rcl_service_t * service = handle2Instance(service_handle); + rcl_ret_t ret = rcl_wait_set_add_service(wait_set, service); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to add service to wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to add service to wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetClearClients(JNIEnv * env, +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetClearClients( + JNIEnv * env, jclass, jlong wait_set_handle) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + rcl_ret_t ret = rcl_wait_set_clear_clients(wait_set); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to clear clients from wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to clear clients from wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetAddClient(JNIEnv * env, jclass, +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeWaitSetAddClient( + JNIEnv * env, + jclass, jlong wait_set_handle, jlong client_handle) { - rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); - rcl_client_t * client = reinterpret_cast(client_handle); + rcl_wait_set_t * wait_set = handle2Instance(wait_set_handle); + rcl_client_t * client = handle2Instance(client_handle); + rcl_ret_t ret = rcl_wait_set_add_client(wait_set, client); if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to add client to wait set: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to add client to wait set: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeRequest(JNIEnv * env, jclass, +JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeRequest( + JNIEnv * env, + jclass, jlong service_handle, jlong jrequest_from_java_converter_handle, - jlong jrequest_to_java_converter_handle, jobject jrequest_msg) + jlong jrequest_to_java_converter_handle, + jobject jrequest_msg) { assert(service_handle != 0); assert(jrequest_from_java_converter_handle != 0); assert(jrequest_to_java_converter_handle != 0); assert(jrequest_msg != nullptr); - rcl_service_t * service = reinterpret_cast(service_handle); + rcl_service_t * service = handle2Instance(service_handle); convert_from_java_signature convert_from_java = reinterpret_cast(jrequest_from_java_converter_handle); @@ -284,17 +384,15 @@ JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeRequest(JNIEnv rmw_request_id_t header; rcl_ret_t ret = rcl_take_request(service, &header, taken_msg); - if (ret != RCL_RET_OK && ret != RCL_RET_SERVICE_TAKE_FAILED) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to take request from a service: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to take request from a service: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); return nullptr; } if (ret != RCL_RET_SERVICE_TAKE_FAILED) { jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); - assert(jtaken_msg != nullptr); jobject jheader = convert_rmw_request_id_to_java(env, &header); @@ -304,10 +402,13 @@ JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeRequest(JNIEnv return nullptr; } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeSendServiceResponse(JNIEnv * env, jclass, +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeSendServiceResponse( + JNIEnv * env, + jclass, jlong service_handle, jobject jrequest_id, - jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_from_java_converter_handle, + jlong jresponse_to_java_converter_handle, jobject jresponse_msg) { assert(service_handle != 0); @@ -315,7 +416,7 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeSendServiceResponse(J assert(jresponse_to_java_converter_handle != 0); assert(jresponse_msg != nullptr); - rcl_service_t * service = reinterpret_cast(service_handle); + rcl_service_t * service = handle2Instance(service_handle); convert_from_java_signature convert_from_java = reinterpret_cast(jresponse_from_java_converter_handle); @@ -325,25 +426,27 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeSendServiceResponse(J rmw_request_id_t * request_id = convert_rmw_request_id_from_java(env, jrequest_id); rcl_ret_t ret = rcl_send_response(service, request_id, response_msg); - if (ret != RCL_RET_OK) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to send response from a service: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to send response from a service: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); } } -JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeResponse(JNIEnv * env, jclass, +JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeResponse( + JNIEnv * env, + jclass, jlong client_handle, jlong jresponse_from_java_converter_handle, - jlong jresponse_to_java_converter_handle, jobject jresponse_msg) + jlong jresponse_to_java_converter_handle, + jobject jresponse_msg) { assert(client_handle != 0); assert(jresponse_from_java_converter_handle != 0); assert(jresponse_to_java_converter_handle != 0); assert(jresponse_msg != nullptr); - rcl_client_t * client = reinterpret_cast(client_handle); + rcl_client_t * client = handle2Instance(client_handle); convert_from_java_signature convert_from_java = reinterpret_cast(jresponse_from_java_converter_handle); @@ -356,11 +459,10 @@ JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeResponse(JNIEn rmw_request_id_t header; rcl_ret_t ret = rcl_take_response(client, &header, taken_msg); - if (ret != RCL_RET_OK && ret != RCL_RET_CLIENT_TAKE_FAILED) { - rcljava_throw_exception( - env, "java/lang/IllegalStateException", - "Failed to take request from a service: " + std::string(rcl_get_error_string_safe())); + std::string message("Failed to take request from a service: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); return nullptr; } @@ -376,66 +478,13 @@ JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_RCLJava_nativeTakeResponse(JNIEn return nullptr; } - -jobject convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) -{ - jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/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_RCLJava_nativeConvertQoSProfileToHandle( - JNIEnv *, jclass, jint history, jint depth, jint reliability, jint durability) + JNIEnv *, + jclass, + jint history, + jint depth, + jint reliability, + jint durability) { rmw_qos_profile_t * qos_profile = static_cast(malloc(sizeof(rmw_qos_profile_t))); @@ -446,7 +495,9 @@ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_RCLJava_nativeConvertQoSProfileToH return reinterpret_cast(qos_profile); } -JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeDisposeQoSProfile(JNIEnv *, jclass, +JNIEXPORT void JNICALL Java_org_ros2_rcljava_RCLJava_nativeDisposeQoSProfile( + JNIEnv *, + jclass, jlong qos_profile_handle) { rmw_qos_profile_t * qos_profile = diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_node_Node.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_node_Node.cpp new file mode 100644 index 00000000..c4945bc0 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_Node.cpp @@ -0,0 +1,317 @@ +// Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard +// +// 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 + +#include "rmw/rmw.h" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/node.h" +#include "rcl/graph.h" +#include "rosidl_generator_c/message_type_support_struct.h" + +#include "rcljava_common/exceptions.h" +#include "rcljava_common/signatures.h" + +#include "rcljava/org_ros2_rcljava_node_Node.h" +#include "rcljava/utils.h" + +/* + * nativeCreatePublisherHandle + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreatePublisherHandle( + JNIEnv * env, + jclass, + jlong jnode_handle, + jclass jmessage_class, + jstring jtopic, + jlong qos_profile_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + rosidl_message_type_support_t * msg_type = jclass2MessageType(env, jmessage_class); + std::string topic = jstring2String(env, jtopic); + + rcl_publisher_t * publisher = makeInstance(); + publisher->impl = NULL; + + rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); + + rmw_qos_profile_t * qos_profile = handle2Instance(qos_profile_handle); + publisher_ops.qos = *qos_profile; + + rcl_ret_t ret = rcl_publisher_init( + publisher, + node, + msg_type, + topic.c_str(), + &publisher_ops); + + if (ret != RCL_RET_OK) { + std::string message("Failed to create publisher: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + + return -1; + } + + jlong publisher_handle = instance2Handle(publisher); + return publisher_handle; +} + +/* + * nativeCreateSubscriptionHandle + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreateSubscriptionHandle( + JNIEnv * env, + jclass, + jlong jnode_handle, + jclass jmessage_class, + jstring jtopic, + jlong qos_profile_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + rosidl_message_type_support_t * msg_type = jclass2MessageType(env, jmessage_class); + std::string topic = jstring2String(env, jtopic); + + rcl_subscription_t * subscription = makeInstance(); + subscription->impl = NULL; + + rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); + rmw_qos_profile_t * qos_profile = handle2Instance(qos_profile_handle); + subscription_ops.qos = *qos_profile; + + rcl_ret_t ret = rcl_subscription_init( + subscription, + node, + msg_type, + topic.c_str(), + &subscription_ops); + + if (ret != RCL_RET_OK) { + std::string message("Failed to create subscription: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + + return -1; + } + + jlong jsubscription = instance2Handle(subscription); + return jsubscription; +} + +/* + * nativeCreateClientHandle + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreateClientHandle( + JNIEnv * env, + jclass, + jlong jnode_handle, + jclass jservice_class, + jstring jservice_topic, + jlong qos_profile_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + rosidl_service_type_support_t * msg_type = jclass2ServiceType(env, jservice_class); + std::string service_topic = jstring2String(env, jservice_topic); + + rcl_client_t * client = makeInstance(); + client->impl = NULL; + +// bool is_available = false; +// rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_available); +// printf("===> is available\n"); +// if (ret != RCL_RET_OK || !is_available) { +// std::string message("Failed to connect to server: " + +// std::string(rcl_get_error_string_safe())); +// throwException(env, message); +// +// return -1; +// } + + rcl_client_options_t client_ops = rcl_client_get_default_options(); + rmw_qos_profile_t * qos_profile = reinterpret_cast(qos_profile_handle); + client_ops.qos = *qos_profile; + + rcl_ret_t ret = rcl_client_init( + client, + node, + msg_type, + service_topic.c_str(), + &client_ops); + + if (ret != RCL_RET_OK) { + std::string message("Failed to create client: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + + return -1; + } + + jlong jclient = instance2Handle(client); + return jclient; +} + +/* + * nativeCreateServiceHandle + */ +JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_node_Node_nativeCreateServiceHandle( + JNIEnv * env, + jclass, + jlong jnode_handle, + jclass jservice_class, + jstring jservice_topic, + jlong qos_profile_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + rosidl_service_type_support_t * msg_type = jclass2ServiceType(env, jservice_class); + std::string service_topic = jstring2String(env, jservice_topic); + + rcl_service_t * service = makeInstance(); + service->impl = NULL; + + rcl_service_options_t service_ops = rcl_service_get_default_options(); + rmw_qos_profile_t * qos_profile = handle2Instance(qos_profile_handle); + service_ops.qos = *qos_profile; + + rcl_ret_t ret = rcl_service_init( + service, + node, + msg_type, + service_topic.c_str(), + &service_ops); + + if (ret != RCL_RET_OK) { + std::string message("Failed to create service: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + + return -1; + } + + jlong jservice = instance2Handle(service); + return jservice; +} + +/* + * + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_Node_nativeDispose( + JNIEnv * env, + jclass, + jlong jnode_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + + rcl_ret_t ret = rcl_node_fini(node); + if (ret != RCL_RET_OK) { + std::string message("Failed finish node: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + } +} + +/* + * + */ +JNIEXPORT jstring JNICALL Java_org_ros2_rcljava_node_Node_nativeGetName( + JNIEnv * env, + jclass, + jlong jnode_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + + const char * name_tmp = rcl_node_get_name(node); + jstring name = env->NewStringUTF(name_tmp); + + return name; +} + +/* + * + */ +JNIEXPORT jint JNICALL Java_org_ros2_rcljava_node_Node_nativeCountPublishers( + JNIEnv * env, + jclass, + jlong jnode_handle, + jstring jtopic) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + std::string topic = jstring2String(env, jtopic); + + size_t count = -1; + rcl_ret_t ret = rcl_count_publishers(node, topic.c_str(), &count); + if (ret != RCL_RET_OK) { + std::string message("Failed to count Publishers: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + } + + return count; +} + +/* + * + */ +JNIEXPORT jint JNICALL Java_org_ros2_rcljava_node_Node_nativeCountSubscribers( + JNIEnv * env, + jclass, + jlong jnode_handle, + jstring jtopic) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + std::string topic = jstring2String(env, jtopic); + + size_t count = 0; + rcl_ret_t ret = rcl_count_subscribers(node, topic.c_str(), &count); + if (ret != RCL_RET_OK) { + std::string message("Failed to count Publishers: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + } + + return count; +} + +JNIEXPORT jobject JNICALL Java_org_ros2_rcljava_node_Node_getListTopics( + JNIEnv * env, + jclass, + jlong jnode_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + rcl_topic_names_and_types_t topic_names_and_types {}; + + rcl_ret_t ret = rcl_get_topic_names_and_types(node, &topic_names_and_types); + if (ret != RCL_RET_OK) { + std::string message("Failed get list of topics: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + } + + jobject topics = makeJTopics(env, &topic_names_and_types); + + ret = rcl_destroy_topic_names_and_types(&topic_names_and_types); + if (ret != RCL_RET_OK) { + std::string message("Failed get list of topics: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + } + + return topics; +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_Client.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_node_service_Client.cpp similarity index 80% rename from rcljava/src/main/cpp/org_ros2_rcljava_Client.cpp rename to rcljava/src/main/cpp/org_ros2_rcljava_node_service_Client.cpp index 0cb57130..39a5aafc 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_Client.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_service_Client.cpp @@ -1,4 +1,5 @@ // Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -27,11 +28,16 @@ #include "rcljava_common/exceptions.h" #include "rcljava_common/signatures.h" -#include "org_ros2_rcljava_Client.h" +#include "rcljava/org_ros2_rcljava_node_service_Client.h" -JNIEXPORT void JNICALL Java_org_ros2_rcljava_Client_nativeSendClientRequest(JNIEnv * env, jclass, - jlong client_handle, jlong sequence_number, jlong jrequest_from_java_converter_handle, - jlong jrequest_to_java_converter_handle, jobject jrequest_msg) +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_service_Client_nativeSendClientRequest( + JNIEnv * env, + jclass, + jlong client_handle, + jlong sequence_number, + jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, + jobject jrequest_msg) { assert(client_handle != 0); assert(jrequest_from_java_converter_handle != 0); diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_node_topic_Publisher.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_node_topic_Publisher.cpp new file mode 100644 index 00000000..a596dd77 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_node_topic_Publisher.cpp @@ -0,0 +1,73 @@ +// Copyright 2016 Esteve Fernandez +// Copyright 2016 Mickael Gaillard +// +// 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 + +#include "rmw/rmw.h" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/node.h" + +#include "rcljava_common/exceptions.h" +#include "rcljava_common/signatures.h" + +#include "rcljava/org_ros2_rcljava_node_topic_Publisher.h" +#include "rcljava/utils.h" + +/* + * nativePublish + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_topic_Publisher_nativePublish( + JNIEnv * env, + jclass, + jlong jpublisher_handle, + jobject jmsg) +{ + rcl_publisher_t * publisher = handle2Instance(jpublisher_handle); + + void * raw_ros_message = jobject2Message(env, jmsg); + + rcl_ret_t ret = rcl_publish(publisher, raw_ros_message); + if (ret != RCL_RET_OK) { + std::string message("Failed to publish: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + } +} + +/* + * nativeDispose + */ +JNIEXPORT void JNICALL Java_org_ros2_rcljava_node_topic_Publisher_nativeDispose( + JNIEnv * env, + jclass, + jlong jnode_handle, + jlong jpublisher_handle) +{ + rcl_node_t * node = handle2Instance(jnode_handle); + rcl_publisher_t * publisher = handle2Instance(jpublisher_handle); + + rcl_ret_t ret = rcl_publisher_fini(publisher, node); + if (ret != RCL_RET_OK) { + std::string message("Failed to destroy publisher: " + + std::string(rcl_get_error_string_safe())); + throwException(env, message); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/Client.java b/rcljava/src/main/java/org/ros2/rcljava/Client.java deleted file mode 100644 index fffed414..00000000 --- a/rcljava/src/main/java/org/ros2/rcljava/Client.java +++ /dev/null @@ -1,133 +0,0 @@ -/* Copyright 2016 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; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import java.lang.ref.WeakReference; -import java.util.HashMap; -import java.util.Map; -import java.util.concurrent.Future; - -public class Client { - private static final Logger logger = LoggerFactory.getLogger(Client.class); - - static { - try { - System.loadLibrary("rcljavaClient__" + RCLJava.getRMWIdentifier()); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } - } - - private final WeakReference nodeReference; - private final long nodeHandle; - private final long clientHandle; - private final Class serviceType; - private final String serviceName; - private long sequenceNumber = 0; - private Map pendingRequests; - - private long requestFromJavaConverterHandle = 0; - private long requestToJavaConverterHandle = 0; - - private long responseFromJavaConverterHandle = 0; - private long responseToJavaConverterHandle = 0; - - private final Class requestType; - private final Class responseType; - - public Client(final WeakReference nodeReference, - final long nodeHandle, final long clientHandle, - final Class serviceType, final String serviceName, - final Class requestType, final Class responseType, - final long requestFromJavaConverterHandle, - final long requestToJavaConverterHandle, - final long responseFromJavaConverterHandle, - final long responseToJavaConverterHandle) { - this.nodeReference = nodeReference; - this.nodeHandle = nodeHandle; - this.clientHandle = clientHandle; - this.serviceType = serviceType; - this.serviceName = serviceName; - this.requestType = requestType; - this.responseType = responseType; - this.requestFromJavaConverterHandle = requestFromJavaConverterHandle; - this.requestToJavaConverterHandle = requestToJavaConverterHandle; - this.responseFromJavaConverterHandle = responseFromJavaConverterHandle; - this.responseToJavaConverterHandle = responseToJavaConverterHandle; - this.pendingRequests = new HashMap(); - } - - public final Future sendRequest(final U request) { - synchronized (pendingRequests) { - sequenceNumber++; - nativeSendClientRequest(clientHandle, sequenceNumber, - requestFromJavaConverterHandle, requestToJavaConverterHandle, - request); - RCLFuture future = new RCLFuture(this.nodeReference); - pendingRequests.put(sequenceNumber, future); - return future; - } - } - - public final void handleResponse(final RMWRequestId header, - final U response) { - synchronized (pendingRequests) { - long sequenceNumber = header.sequenceNumber; - RCLFuture future = pendingRequests.remove(sequenceNumber); - future.set(response); - } - } - - public final Class getServiceType() { - return serviceType; - } - - public final long getClientHandle() { - return clientHandle; - } - - private static native void nativeSendClientRequest(long clientHandle, - long sequenceNumber, long requestFromJavaConverterHandle, - long requestToJavaConverterHandle, Object requestMessage); - - public final long getRequestFromJavaConverterHandle() { - return this.requestFromJavaConverterHandle; - } - - public final long getRequestToJavaConverterHandle() { - return this.requestToJavaConverterHandle; - } - - public final long getResponseFromJavaConverterHandle() { - return this.responseFromJavaConverterHandle; - } - - public final long getResponseToJavaConverterHandle() { - return this.responseToJavaConverterHandle; - } - - public final Class getRequestType() { - return this.requestType; - } - - public final Class getResponseType() { - return this.responseType; - } -} diff --git a/rcljava/src/main/java/org/ros2/rcljava/Log.java b/rcljava/src/main/java/org/ros2/rcljava/Log.java new file mode 100644 index 00000000..c7179a1b --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/Log.java @@ -0,0 +1,195 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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; + +//import java.io.PrintWriter; +//import java.io.StringWriter; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.topic.Publisher; +import org.ros2.rcljava.node.topic.Topics; + +import std_msgs.msg.String; + +/** + * Not define in ROS2. Copy of ROS1 + * + */ +public class Log { + + private final Logger log; + private final Node defaultNode; + private final Publisher publisher; + + private boolean isDebugEnabled; + private boolean isErrorEnabled; + private boolean isFatalEnabled; + private boolean isInfoEnabled; + private boolean isTraceEnabled; + private boolean isWarnEnabled; + + public Log(final Node defaultNode) { + this.defaultNode = defaultNode; + this.publisher = this.defaultNode.createPublisher(std_msgs.msg.String.class, Topics.ROSOUT); + this.log = LoggerFactory.getLogger(defaultNode.getClass()); + } + +// public Publisher getPublisher() { +// return this.publisher; +// } + +// private void publish(byte level, Object message, Throwable throwable) { +// StringWriter stringWriter = new StringWriter(); +// PrintWriter printWriter = new PrintWriter(stringWriter); +// throwable.printStackTrace(printWriter); +// this.publish(level, message.toString() + '\n' + stringWriter.toString()); +// } + + private void publish(byte level, Object message) { +// rosgraph_msgs.msg.Log logMessage = new rosgraph_msgs.msg.Log(); +// logMessage.getHeader().setStamp(this.defaultNode.getCurrentTime()); +// logMessage.setLevel(level); +// logMessage.setName(this.defaultNode.getName()); +// logMessage.setMsg(message.toString()); + String logMessage = new String(); + logMessage.setData(message.toString()); + this.publisher.publish(logMessage); + } + + public boolean isDebugEnabled() { + return this.isDebugEnabled; + } + + public boolean isErrorEnabled() { + return this.isErrorEnabled; + } + + public boolean isFatalEnabled() { + return this.isFatalEnabled; + } + + public boolean isInfoEnabled() { + return this.isInfoEnabled; + } + + public boolean isTraceEnabled() { + return this.isTraceEnabled; + } + + public boolean isWarnEnabled() { + return this.isWarnEnabled; + } + + public void trace(Object message) { + this.log.trace(message.toString()); + if (this.isTraceEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.DEBUG, message); + } + } + + public void trace(Object message, Throwable t) { + this.log.trace(message.toString()); + if (this.isTraceEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.DEBUG, message, t); + } + } + + public void debug(Object message) { + this.log.debug(message.toString()); + if (this.isDebugEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.DEBUG, message); + } + } + + public void debug(Object message, Throwable t) { + this.log.debug(message.toString()); + if (this.isDebugEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.DEBUG, message, t); + } + } + + public void info(Object message) { + this.log.info(message.toString()); + if (this.isInfoEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.INFO, message); + } + } + + public void info(Object message, Throwable t) { + this.log.info(message.toString()); + if (this.isInfoEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.INFO, message, t); + } + } + + public void warn(Object message) { + this.log.warn(message.toString()); + if (this.isWarnEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.WARN, message); + } + } + + public void warn(Object message, Throwable t) { + this.log.warn(message.toString()); + if (this.isWarnEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.msg.Log.WARN, message, t); + } + } + + public void error(Object message) { + this.log.error(message.toString()); + if (this.isErrorEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.ERROR, message); + } + } + + public void error(Object message, Throwable t) { + this.log.error(message.toString()); + if (this.isErrorEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.ERROR, message, t); + } + } + + public void fatal(Object message) { + this.log.error(message.toString()); + if (this.isFatalEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.FATAL, message); + } + } + + public void fatal(Object message, Throwable t) { + this.log.error(message.toString()); + if (this.isFatalEnabled() && this.publisher != null) { + this.publish((byte)0, message); +// this.publish(rosgraph_msgs.Log.FATAL, message, t); + } + } + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/NativeUtils.java b/rcljava/src/main/java/org/ros2/rcljava/NativeUtils.java new file mode 100644 index 00000000..ffbc0232 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/NativeUtils.java @@ -0,0 +1,52 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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; + +import java.util.Vector; + +/** + * Utility for load JNI. + */ +public abstract class NativeUtils { + + private static java.lang.reflect.Field LIBRARIES; + + static { + try { + LIBRARIES = ClassLoader.class.getDeclaredField("loadedLibraryNames"); + LIBRARIES.setAccessible(true); + } catch (NoSuchFieldException e) { + e.printStackTrace(); + } catch (SecurityException e) { + e.printStackTrace(); + } + } + + @SuppressWarnings("unchecked") + public static String[] getLoadedLibraries(final ClassLoader loader) { + Vector libraries = new Vector(); + + try { + libraries = (Vector) LIBRARIES.get(loader); + } catch (IllegalArgumentException e) { + e.printStackTrace(); + } catch (IllegalAccessException e) { + e.printStackTrace(); + } + + return libraries.toArray(new String[] {}); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/Node.java b/rcljava/src/main/java/org/ros2/rcljava/Node.java deleted file mode 100644 index e9e46c08..00000000 --- a/rcljava/src/main/java/org/ros2/rcljava/Node.java +++ /dev/null @@ -1,339 +0,0 @@ -/* Copyright 2016 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; - -import org.ros2.rcljava.qos.QoSProfile; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -import java.lang.ref.WeakReference; -import java.lang.reflect.InvocationTargetException; -import java.lang.reflect.Method; -import java.util.Queue; -import java.util.concurrent.LinkedBlockingQueue; - -/** - * This class serves as a bridge between ROS2's rcl_node_t and RCLJava. - * A Node must be created via @{link RCLJava#createNode(String)} - */ -public class Node { - - private static final Logger logger = LoggerFactory.getLogger(Node.class); - - static { - try { - System.loadLibrary("rcljavaNode__" + RCLJava.getRMWIdentifier()); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } - } - - /** - * An integer that represents a pointer to the underlying ROS2 node - * structure (rcl_node_t). - */ - private final long nodeHandle; - - /** - * All the @{link Subscription}s that have been created through this instance. - */ - private final Queue subscriptions; - - /** - * All the @{link Publisher}s that have been created through this instance. - */ - private final Queue publishers; - - /** - * All the @{link Service}s that have been created through this instance. - */ - private final Queue services; - - /** - * All the @{link Client}s that have been created through this instance. - */ - private final Queue clients; - - /** - * Constructor. - * - * @param nodeHandle A pointer to the underlying ROS2 node structure. Must not - * be zero. - */ - public Node(final long nodeHandle) { - this.nodeHandle = nodeHandle; - this.publishers = new LinkedBlockingQueue(); - this.subscriptions = new LinkedBlockingQueue(); - this.services = new LinkedBlockingQueue(); - this.clients = new LinkedBlockingQueue(); - } - - /** - * Create a ROS2 publisher (rcl_publisher_t) and return a pointer to - * it as an integer. - * - * @param The type of the messages that will be published by the - * created @{link Publisher}. - * @param nodeHandle A pointer to the underlying ROS2 node structure. - * @param messageType The class of the messages that will be published by the - * created @{link Publisher}. - * @param topic The topic to which the created @{link Publisher} will - * publish messages. - * @param qosProfileHandle A pointer to the underlying ROS2 QoS profile - * structure. - * @return A pointer to the underlying ROS2 publisher structure. - */ - private static native long nativeCreatePublisherHandle( - long nodeHandle, Class messageType, String topic, - long qosProfileHandle); - - /** - * Create a ROS2 subscription (rcl_subscription_t) and return a pointer to - * it as an integer. - * - * @param The type of the messages that will be received by the - * created @{link Subscription}. - * @param nodeHandle A pointer to the underlying ROS2 node structure. - * @param messageType The class of the messages that will be received by the - * created @{link Subscription}. - * @param topic The topic from which the created @{link Subscription} will - * receive messages. - * @param qosProfileHandle A pointer to the underlying ROS2 QoS profile - * structure. - * @return A pointer to the underlying ROS2 subscription structure. - */ - private static native long nativeCreateSubscriptionHandle( - long nodeHandle, Class messageType, String topic, - long qosProfileHandle); - - /** - * Create a Publisher<T>. - * - * @param The type of the messages that will be published by the - * created @{link Publisher}. - * @param messageType The class of the messages that will be published by the - * created @{link Publisher}. - * @param topic The topic to which the created @{link Publisher} will - * publish messages. - * @return A @{link Publisher} that represents the underlying ROS2 publisher - * structure. - */ - public final Publisher createPublisher( - final Class messageType, final String topic, - final QoSProfile qosProfile) { - - long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); - long publisherHandle = nativeCreatePublisherHandle(this.nodeHandle, - messageType, topic, qosProfileHandle); - RCLJava.disposeQoSProfile(qosProfileHandle); - - Publisher publisher = new Publisher(this.nodeHandle, - publisherHandle, topic); - this.publishers.add(publisher); - - return publisher; - } - - public final Publisher createPublisher( - final Class messageType, final String topic) { - return this.createPublisher(messageType, topic, QoSProfile.DEFAULT); - } - - /** - * Create a Subscription<T>. - * - * @param The type of the messages that will be received by the - * created @{link Subscription}. - * @param messageType The class of the messages that will be received by the - * created @{link Subscription}. - * @param topic The topic from which the created @{link Subscription} will - * receive messages. - * @param callback The callback function that will be triggered when a - * message is received by the @{link Subscription}. - * @return A @{link Subscription} that represents the underlying ROS2 - * subscription structure. - */ - public final Subscription createSubscription( - final Class messageType, final String topic, - final Consumer callback, final QoSProfile qosProfile) { - - long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); - long subscriptionHandle = nativeCreateSubscriptionHandle( - this.nodeHandle, messageType, topic, qosProfileHandle); - RCLJava.disposeQoSProfile(qosProfileHandle); - - Subscription subscription = new Subscription( - this.nodeHandle, subscriptionHandle, messageType, topic, callback); - - this.subscriptions.add(subscription); - - return subscription; - } - - public final Subscription createSubscription( - final Class messageType, final String topic, - final Consumer callback) { - return this.createSubscription(messageType, topic, callback, - QoSProfile.DEFAULT); - } - - /** - * @return All the @{link Subscription}s that were created by this instance. - */ - public final Queue getSubscriptions() { - return this.subscriptions; - } - - /** - * @return All the @{link Publisher}s that were created by this instance. - */ - public final Queue getPublishers() { - return this.publishers; - } - - /** - * Safely destroy the underlying ROS2 node structure. - */ - public final void dispose() { - // TODO(esteve): implement - } - - public final long getNodeHandle() { - return this.nodeHandle; - } - - private static native long nativeCreateServiceHandle( - long nodeHandle, 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, NoSuchMethodException, - InvocationTargetException { - - Class requestType = (Class) serviceType.getField("RequestType").get(null); - - Method requestFromJavaConverterMethod = requestType.getDeclaredMethod( - "getFromJavaConverter", (Class[]) null); - long requestFromJavaConverterHandle = - (Long) requestFromJavaConverterMethod.invoke(null, (Class[]) null); - - Method requestToJavaConverterMethod = requestType.getDeclaredMethod( - "getToJavaConverter", (Class[]) null); - long requestToJavaConverterHandle = - (Long) requestToJavaConverterMethod.invoke(null, (Class[]) null); - - Class responseType = (Class) serviceType.getField("ResponseType").get(null); - - Method responseFromJavaConverterMethod = responseType.getDeclaredMethod( - "getFromJavaConverter", (Class[]) null); - long responseFromJavaConverterHandle = - (Long) responseFromJavaConverterMethod.invoke(null, (Class[]) null); - - Method responseToJavaConverterMethod = responseType.getDeclaredMethod( - "getToJavaConverter", (Class[]) null); - long responseToJavaConverterHandle = - (Long) responseToJavaConverterMethod.invoke(null, (Class[]) null); - - long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); - long serviceHandle = nativeCreateServiceHandle(this.nodeHandle, serviceType, serviceName, - qosProfileHandle); - RCLJava.disposeQoSProfile(qosProfileHandle); - - Service service = new Service(this.nodeHandle, serviceHandle, - serviceType, serviceName, callback, requestType, responseType, - requestFromJavaConverterHandle, requestToJavaConverterHandle, - responseFromJavaConverterHandle, responseToJavaConverterHandle); - this.services.add(service); - - return service; - } - - public Service createService(final Class serviceType, - final String serviceName, final TriConsumer callback) - throws NoSuchFieldException, - IllegalAccessException, NoSuchMethodException, - InvocationTargetException { - return this.createService(serviceType, serviceName, callback, - QoSProfile.SERVICES_DEFAULT); - } - - public final Queue getServices() { - return this.services; - } - - public final Client createClient(final Class serviceType, - final String serviceName, final QoSProfile qosProfile) throws - NoSuchFieldException, IllegalAccessException, NoSuchMethodException, - InvocationTargetException { - - Class requestType = (Class) serviceType.getField("RequestType").get(null); - - Method requestFromJavaConverterMethod = requestType.getDeclaredMethod( - "getFromJavaConverter", (Class[]) null); - long requestFromJavaConverterHandle = - (Long) requestFromJavaConverterMethod.invoke(null, (Class[]) null); - - Method requestToJavaConverterMethod = requestType.getDeclaredMethod( - "getToJavaConverter", (Class[]) null); - long requestToJavaConverterHandle = - (Long) requestToJavaConverterMethod.invoke(null, (Class[]) null); - - Class responseType = (Class) serviceType.getField("ResponseType").get(null); - - Method responseFromJavaConverterMethod = responseType.getDeclaredMethod( - "getFromJavaConverter", (Class[]) null); - long responseFromJavaConverterHandle = - (Long) responseFromJavaConverterMethod.invoke(null, (Class[]) null); - - Method responseToJavaConverterMethod = responseType.getDeclaredMethod( - "getToJavaConverter", (Class[]) null); - long responseToJavaConverterHandle = - (Long) responseToJavaConverterMethod.invoke(null, (Class[]) null); - - long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); - long clientHandle = nativeCreateClientHandle(this.nodeHandle, serviceType, - serviceName, qosProfileHandle); - RCLJava.disposeQoSProfile(qosProfileHandle); - - Client client = new Client(new WeakReference(this), - this.nodeHandle, clientHandle, serviceType, serviceName, requestType, - responseType, requestFromJavaConverterHandle, - requestToJavaConverterHandle, responseFromJavaConverterHandle, - responseToJavaConverterHandle); - this.clients.add(client); - - return client; - } - - public Client createClient(final Class serviceType, - final String serviceName) throws NoSuchFieldException, - IllegalAccessException, NoSuchMethodException, InvocationTargetException { - return this.createClient(serviceType, serviceName, - QoSProfile.SERVICES_DEFAULT); - } - - private static native long nativeCreateClientHandle( - long nodeHandle, Class cls, String serviceName, - long qosProfileHandle); - - public final Queue getClients() { - return this.clients; - } -} diff --git a/rcljava/src/main/java/org/ros2/rcljava/Publisher.java b/rcljava/src/main/java/org/ros2/rcljava/Publisher.java deleted file mode 100644 index 7976659e..00000000 --- a/rcljava/src/main/java/org/ros2/rcljava/Publisher.java +++ /dev/null @@ -1,119 +0,0 @@ -/* Copyright 2016 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; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -/** - * This class serves as a bridge between ROS2's rcl_publisher_t and RCLJava. - * A Publisher must be created via - * @{link Node#createPublisher(Class<T>, String)} - * - * @param The type of the messages that this publisher will publish. - */ -public class Publisher { - - private static final Logger logger = LoggerFactory.getLogger(Publisher.class); - - static { - try { - System.loadLibrary("rcljavaPublisher__" + RCLJava.getRMWIdentifier()); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } - } - - /** - * An integer that represents a pointer to the underlying ROS2 node - * structure (rcl_node_t). - */ - private final long nodeHandle; - - /** - * An integer that represents a pointer to the underlying ROS2 publisher - * structure (rcl_publisher_t). - */ - private final long publisherHandle; - - /** - * The topic to which this publisher will publish messages. - */ - private final String topic; - - /** - * Constructor. - * - * @param nodeHandle A pointer to the underlying ROS2 node structure that - * created this subscription, as an integer. Must not be zero. - * @param publisherHandle A pointer to the underlying ROS2 publisher - * structure, as an integer. Must not be zero. - * @param topic The topic to which this publisher will publish messages. - */ - public Publisher(final long nodeHandle, final long publisherHandle, - final String topic) { - this.nodeHandle = nodeHandle; - this.publisherHandle = publisherHandle; - this.topic = topic; - } - - /** - * Publish a message via the underlying ROS2 mechanisms. - * - * @param The type of the messages that this publisher will publish. - * @param publisherHandle A pointer to the underlying ROS2 publisher - * structure, as an integer. Must not be zero. - * @param message An instance of the <T> parameter. - */ - private static native void nativePublish( - long publisherHandle, T message); - - /** - * Publish a message. - * - * @param message An instance of the <T> parameter. - */ - public final void publish(final T message) { - nativePublish(this.publisherHandle, message); - } - - /** - * Destroy a ROS2 publisher (rcl_publisher_t). - * - * @param nodeHandle A pointer to the underlying ROS2 node structure that - * created this subscription, as an integer. Must not be zero. - * @param publisherHandle A pointer to the underlying ROS2 publisher - * structure, as an integer. Must not be zero. - */ - private static native void nativeDispose( - long nodeHandle, long publisherHandle); - - /** - * Safely destroy the underlying ROS2 publisher structure. - */ - public final void dispose() { - nativeDispose(this.nodeHandle, this.publisherHandle); - } - - public final long getNodeHandle() { - return this.nodeHandle; - } - - public final long getPublisherHandle() { - return this.publisherHandle; - } -} diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java index 70729988..5040ae50 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java +++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,357 +13,543 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava; -import org.ros2.rcljava.qos.QoSProfile; +import java.util.List; +import java.util.Map; +import java.util.concurrent.ConcurrentSkipListMap; import org.slf4j.Logger; import org.slf4j.LoggerFactory; -import java.util.Map; -import java.util.Queue; -import java.util.concurrent.ConcurrentSkipListMap; -import java.util.concurrent.LinkedBlockingQueue; +import org.ros2.rcljava.exception.ImplementationAlreadyImportedException; +import org.ros2.rcljava.exception.NoImplementationAvailableException; +import org.ros2.rcljava.exception.NotInitializedException; +import org.ros2.rcljava.internal.message.Message; +import org.ros2.rcljava.namespace.GraphName; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.service.Client; +import org.ros2.rcljava.node.service.RMWRequestId; +import org.ros2.rcljava.node.service.Service; +import org.ros2.rcljava.node.topic.Subscription; +import org.ros2.rcljava.qos.QoSProfile; /** - * Entry point for the ROS2 Java API, similar to the rclcpp API. + * ROS2 java client wrapper. + * + *

JNI call of ROS2 c client.

+ * */ -public final class RCLJava { - - private static final Logger logger = LoggerFactory.getLogger(RCLJava.class); - - /** - * Private constructor so this cannot be instantiated. - */ - private RCLJava() { } - - /** - * All the @{link Node}s that have been created. - */ - private static Queue nodes; - - static { - nodes = new LinkedBlockingQueue(); - - RMW_TO_TYPESUPPORT = new ConcurrentSkipListMap() {{ - put("rmw_fastrtps_cpp", "rosidl_typesupport_introspection_c"); - put("rmw_opensplice_cpp", "rosidl_typesupport_opensplice_c"); - put("rmw_connext_cpp", "rosidl_typesupport_connext_c"); - put("rmw_connext_dynamic_cpp", "rosidl_typesupport_introspection_c"); - } +public class RCLJava { + + private static final String LOG_NAME = RCLJava.class.getName(); + + private static final Logger logger = LoggerFactory.getLogger(RCLJava.class); + + /** + * The identifier of the currently active RMW implementation. + */ + private static String rmwImplementation = null; + + /** + * Flag to indicate if RCLJava has been fully initialized, with a valid RMW + * implementation. + */ + private static boolean initialized = false; + + /** + * A mapping between RMW implementations and their typesupports. + */ + private static final Map RMW_TO_TYPESUPPORT = new ConcurrentSkipListMap() { + /** Serial Id */ + private static final long serialVersionUID = 1L; + + { + put("rmw_fastrtps_cpp", "rosidl_typesupport_introspection_c"); + put("rmw_opensplice_cpp", "rosidl_typesupport_opensplice_c"); + put("rmw_connext_cpp", "rosidl_typesupport_connext_c"); + put("rmw_connext_dynamic_cpp", "rosidl_typesupport_introspection_c"); + } }; - Runtime.getRuntime().addShutdownHook(new Thread() { - public void run() { - for (Node node : nodes) { - node.dispose(); - } - } - }); - } - - /** - * The identifier of the currently active RMW implementation. - */ - private static String rmwImplementation = null; - - /** - * Flag to indicate if RCLJava has been fully initialized, with a valid RMW - * implementation. - */ - private static boolean initialized = false; - - /** - * A mapping between RMW implementations and their typesupports. - */ - private static final Map RMW_TO_TYPESUPPORT; - - /** - * @return true if RCLJava has been fully initialized, false otherwise. - */ - public static boolean isInitialized() { - return RCLJava.initialized; - } - - /** - * Initialize the RCLJava API. If successful, a valid RMW implementation will - * be loaded and accessible, enabling the creating of ROS2 entities - * (@{link Node}s, @{link Publisher}s and @{link Subscription}s. - */ - public static void rclJavaInit() { - synchronized (RCLJava.class) { - if (!initialized) { - if (RCLJava.rmwImplementation == null) { - for (Map.Entry entry - : RMW_TO_TYPESUPPORT.entrySet()) { + // Natives definitions + //################################################################################################################// + + // rcl.h + /** + * Initialize the underlying rcl layer. + */ + private static native void nativeRCLJavaInit(String args[]); + + private static native void nativeShutdown(); + + /** + * Call the underlying ROS2 rcl mechanism to check if ROS2 has been shut + * down. + * + * @return true if RCLJava hasn't been shut down, false otherwise. + */ + private static native boolean nativeOk(); + + /** + * @return The identifier of the currently active RMW implementation via the + * native ROS2 API. + */ + private static native String nativeGetRMWIdentifier(); + + // Node.h + /** + * Create a ROS2 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. + * @return A pointer to the underlying ROS2 node structure. + */ + private static native long nativeCreateNodeHandle(String nodeName); + + // Wait.h + private static native long nativeGetZeroInitializedWaitSet(); + private static native void nativeWaitSetInit( + long waitSetHandle, + int numberOfSubscriptions, + int numberOfGuardConditions, + int numberOfTimers, + int numberOfClients, + int numberOfServices); + private static native void nativeWaitSetClearSubscriptions(long waitSetHandle); + private static native void nativeWaitSetAddSubscription(long waitSetHandle, long subscriptionHandle); + private static native void nativeWaitSetClearServices(long waitSetHandle); + private static native void nativeWaitSetAddService(long waitSetHandle, long serviceHandle); + private static native void nativeWaitSetClearClients(long waitSetHandle); + private static native void nativeWaitSetAddClient(long waitSetHandle, long clientHandle); + private static native void nativeWait(long waitSetHandle); + private static native Message nativeTake(long SubscriptionHandle, Class msgType); + private static native void nativeWaitSetFini(long waitSetHandle); + private static native List nativeGetNodeNames(); + private static native Object nativeTakeRequest( + long serviceHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + Object requestMessage); + private static native void nativeSendServiceResponse( + long serviceHandle, + Object header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + Object responseMessage); + private static native Object nativeTakeResponse( + long clientHandle, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + Object responseMessage); + private static native long nativeConvertQoSProfileToHandle( + int history, int depth, int reliability, int durability); + private static native void nativeDisposeQoSProfile( + long qosProfileHandle); + + /** Release all ressources at shutdown. */ + static { + Runtime.getRuntime().addShutdownHook(new Thread() { + public void run() { + if (RCLJava.initialized) { + RCLJava.logger.debug("Final Shutdown..."); + + // List loaded libraries. + String[] list = NativeUtils.getLoadedLibraries(RCLJava.class.getClassLoader()); + StringBuilder msgLog = new StringBuilder(); + for (String key : list) { + msgLog.append(key); + msgLog.append("\n"); + } + RCLJava.logger.debug("Native libraries Loaded: \n" + msgLog.toString()); + } + + GraphName.dispose(); + } + }); + } - try { - setRMWImplementation(entry.getKey()); - break; - } catch (UnsatisfiedLinkError ule) { - // TODO(esteve): handle exception - } catch (Exception exc) { - // TODO(esteve): handle exception + /** + * Private constructor so this cannot be instantiated. + */ + private RCLJava() { } + + /** + * Initialize the RCLJava API. If successful, a valid RMW implementation will + * be loaded and accessible, enabling the creating of ROS2 entities + * (@{link Node}s, @{link Publisher}s and @{link Subscription}s. + */ + public static void rclJavaInit(final String args[]) { + synchronized (RCLJava.class) { + if (!RCLJava.initialized) { + if (RCLJava.rmwImplementation == null) { + String libpath = System.getProperty("java.library.path"); + RCLJava.logger.debug("Native Library path : \n" + libpath.replace(':', '\n')); + + RCLJava.autoLoadRmw(); + } + + if (RCLJava.rmwImplementation == null) { + RCLJava.logger.error("No RMW implementation found..."); + System.exit(1); + } else { + RCLJava.logger.debug("Initialize rclJava with " + RCLJava.rmwImplementation); + RCLJava.nativeRCLJavaInit(args); + RCLJava.initialized = true; + } + } else { + NotInitializedException ex = new NotInitializedException("Cannot intialized twice !"); + logger.error(ex.getMessage()); + throw ex; } - } } - if (RCLJava.rmwImplementation == null) { - logger.error("No RMW implementation found"); - System.exit(1); - } else { - nativeRCLJavaInit(); - logger.info("Using RMW implementation: {}", getRMWIdentifier()); - initialized = true; - } - } - } - } - - /** - * Initialize the underlying rcl layer. - */ - private static native void nativeRCLJavaInit(); - - /** - * Create a ROS2 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. - * @return A pointer to the underlying ROS2 node structure. - */ - private static native long nativeCreateNodeHandle(String nodeName); - - public static String getTypesupportIdentifier() { - return RMW_TO_TYPESUPPORT.get(nativeGetRMWIdentifier()); - } - - public static void setRMWImplementation( - final String rmwImplementation) throws Exception { - - synchronized (RCLJava.class) { - System.loadLibrary("rcljavaRCLJava__" + rmwImplementation); - RCLJava.rmwImplementation = rmwImplementation; } - } - - /** - * @return The identifier of the currently active RMW implementation via the - * native ROS2 API. - */ - private static native String nativeGetRMWIdentifier(); - - /** - * @return The identifier of the currently active RMW implementation. - */ - public static String getRMWIdentifier() { - return nativeGetRMWIdentifier(); - } - - /** - * Call the underlying ROS2 rcl mechanism to check if ROS2 has been shut - * down. - * - * @return true if RCLJava hasn't been shut down, false otherwise. - */ - private static native boolean nativeOk(); - - /** - * @return true if RCLJava hasn't been shut down, false otherwise. - */ - public static boolean ok() { - return nativeOk(); - } - - /** - * 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. - */ - public static Node createNode(final String nodeName) { - long nodeHandle = nativeCreateNodeHandle(nodeName); - Node node = new Node(nodeHandle); - nodes.add(node); - return node; - } - - public static void spinOnce(final Node node) { - long waitSetHandle = nativeGetZeroInitializedWaitSet(); - - nativeWaitSetInit(waitSetHandle, node.getSubscriptions().size(), 0, 0, - node.getClients().size(), node.getServices().size()); - - nativeWaitSetClearSubscriptions(waitSetHandle); - - nativeWaitSetClearServices(waitSetHandle); - - nativeWaitSetClearClients(waitSetHandle); - - for (Subscription subscription : node.getSubscriptions()) { - nativeWaitSetAddSubscription( - waitSetHandle, subscription.getSubscriptionHandle()); + + /** + * @return true if RCLJava has been fully initialized, false otherwise. + */ + public static boolean isInitialized() { + return RCLJava.initialized; } - for (Service service : node.getServices()) { - nativeWaitSetAddService(waitSetHandle, service.getServiceHandle()); + public static void rclJavaInit() { + RCLJava.rclJavaInit(null); } - for (Client client : node.getClients()) { - nativeWaitSetAddClient(waitSetHandle, client.getClientHandle()); + /** + * Create a @{link Node}. + * + * @param nodeName Name of the node. + * @return A @{link Node} that represents the underlying ROS2 node + * structure. + */ + public static Node createNode(final String nodeName) { + return RCLJava.createNode(null, nodeName); } - nativeWait(waitSetHandle); + /** + * Create a @{link Node}. + * + * @param ns Name Space. + * @param nodeName The name that will identify this node in a ROS2 graph. + * @return A @{link Node} that represents the underlying ROS2 node + * structure. + */ + public static Node createNode(final String ns, final String nodeName) { + RCLJava.logger.debug("Create Node stack : " + nodeName); + + if (!RCLJava.initialized) { + throw new NotInitializedException(); + } - for (Subscription subscription : node.getSubscriptions()) { - Object message = nativeTake( - subscription.getSubscriptionHandle(), - subscription.getMessageType()); - if (message != null) { - subscription.getCallback().accept(message); - } - } + long nodeHandle = RCLJava.nativeCreateNodeHandle(nodeName); + Node node = new Node(nodeHandle, ns, nodeName); - for (Service service : node.getServices()) { - long requestFromJavaConverterHandle = - service.getRequestFromJavaConverterHandle(); - long requestToJavaConverterHandle = - service.getRequestToJavaConverterHandle(); - long responseFromJavaConverterHandle = - service.getResponseFromJavaConverterHandle(); - long responseToJavaConverterHandle = - service.getResponseToJavaConverterHandle(); - - Class requestType = service.getRequestType(); - Class responseType = service.getResponseType(); - - Object requestMessage = null; - Object responseMessage = null; - - try { - requestMessage = requestType.newInstance(); - responseMessage = responseType.newInstance(); - } catch (InstantiationException ie) { - ie.printStackTrace(); - continue; - } catch (IllegalAccessException iae) { - iae.printStackTrace(); - continue; - } - - RMWRequestId rmwRequestId = - (RMWRequestId) nativeTakeRequest(service.getServiceHandle(), - requestFromJavaConverterHandle, requestToJavaConverterHandle, - requestMessage); - if (rmwRequestId != null) { - service.getCallback().accept(rmwRequestId, requestMessage, - responseMessage); - nativeSendServiceResponse(service.getServiceHandle(), rmwRequestId, - responseFromJavaConverterHandle, responseToJavaConverterHandle, - responseMessage); - } + return node; } - for (Client client : node.getClients()) { - long requestFromJavaConverterHandle = - client.getRequestFromJavaConverterHandle(); - long requestToJavaConverterHandle = - client.getRequestToJavaConverterHandle(); - long responseFromJavaConverterHandle = - client.getResponseFromJavaConverterHandle(); - long responseToJavaConverterHandle = - client.getResponseToJavaConverterHandle(); - - Class requestType = client.getRequestType(); - Class responseType = client.getResponseType(); - - Object requestMessage = null; - Object responseMessage = null; - - try { - requestMessage = requestType.newInstance(); - responseMessage = responseType.newInstance(); - } catch (InstantiationException ie) { - ie.printStackTrace(); - continue; - } catch (IllegalAccessException iae) { - iae.printStackTrace(); - continue; - } - - RMWRequestId rmwRequestId = (RMWRequestId)nativeTakeResponse( - client.getClientHandle(), responseFromJavaConverterHandle, - responseToJavaConverterHandle, responseMessage); - - if (rmwRequestId != null) { - client.handleResponse(rmwRequestId, responseMessage); - } + + /** + * Wait for once loop. + * + * @param node + */ + @SuppressWarnings({ "unchecked", "rawtypes" }) + public static void spinOnce(final Node node) { + if (!RCLJava.initialized) { + throw new NotInitializedException(); + } + + if (node.getClients().size() > 0 || + node.getPublishers().size() > 0 || + node.getServices().size() > 0 || + node.getSubscriptions().size() > 0) { + long waitSetHandle = RCLJava.nativeGetZeroInitializedWaitSet(); + + RCLJava.nativeWaitSetInit( + waitSetHandle, + node.getSubscriptions().size(), + 0, + 0, + node.getClients().size(), + node.getServices().size()); + + // Clean Waitset components. + RCLJava.nativeWaitSetClearSubscriptions(waitSetHandle); + RCLJava.nativeWaitSetClearServices(waitSetHandle); + RCLJava.nativeWaitSetClearClients(waitSetHandle); + + // Subscribe waiset components. + for (Subscription subscription : node.getSubscriptions()) { + RCLJava.nativeWaitSetAddSubscription(waitSetHandle, subscription.getSubscriptionHandle()); + } + + for (Service service : node.getServices()) { + RCLJava.nativeWaitSetAddService(waitSetHandle, service.getServiceHandle()); + } + + for (Client client : node.getClients()) { + RCLJava.nativeWaitSetAddClient(waitSetHandle, client.getClientHandle()); + } + + // Wait... + RCLJava.nativeWait(waitSetHandle); + RCLJava.nativeWaitSetFini(waitSetHandle); + + // Take all components. + for (Subscription subscription : node.getSubscriptions()) { + Message message = RCLJava.nativeTake(subscription.getSubscriptionHandle(), subscription.getMessageType()); + if (message != null) { + subscription.getCallback().accept(message); + } + } + + for (Service service : node.getServices()) { + long requestFromJavaConverterHandle = service.getRequestFromJavaConverterHandle(); + long requestToJavaConverterHandle = service.getRequestToJavaConverterHandle(); + long responseFromJavaConverterHandle = service.getResponseFromJavaConverterHandle(); + long responseToJavaConverterHandle = service.getResponseToJavaConverterHandle(); + + Class requestType = service.getRequestType(); + Class responseType = service.getResponseType(); + + Object requestMessage = null; + Object responseMessage = null; + + try { + requestMessage = requestType.newInstance(); + responseMessage = responseType.newInstance(); + } catch (InstantiationException ie) { + ie.printStackTrace(); + continue; + } catch (IllegalAccessException iae) { + iae.printStackTrace(); + continue; + } + + RMWRequestId rmwRequestId = (RMWRequestId) RCLJava.nativeTakeRequest( + service.getServiceHandle(), + requestFromJavaConverterHandle, + requestToJavaConverterHandle, + requestMessage); + + if (rmwRequestId != null) { + service.getCallback().accept(rmwRequestId, requestMessage, responseMessage); + RCLJava.nativeSendServiceResponse(service.getServiceHandle(), rmwRequestId, responseFromJavaConverterHandle, + responseToJavaConverterHandle, responseMessage); + } + } + + for (Client client : node.getClients()) { + long responseFromJavaConverterHandle = client.getResponseFromJavaConverterHandle(); + long responseToJavaConverterHandle = client.getResponseToJavaConverterHandle(); + + Class responseType = client.getResponseType(); + + Message responseMessage = null; + + try { + responseMessage = (Message)responseType.newInstance(); + } catch (InstantiationException ie) { + ie.printStackTrace(); + continue; + } catch (IllegalAccessException iae) { + iae.printStackTrace(); + continue; + } + + RMWRequestId rmwRequestId = (RMWRequestId) RCLJava.nativeTakeResponse( + client.getClientHandle(), + responseFromJavaConverterHandle, + responseToJavaConverterHandle, + responseMessage); + + + if (rmwRequestId != null) { + client.handleResponse(rmwRequestId, responseMessage); + } + } + } else { + // TODO fix to rate sleep. + try { + Thread.sleep(1000); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } } - } - private static native void nativeShutdown(); + /** + * Helper Spin. + * @param node + */ + public static void spin(final Node node) { + while(RCLJava.ok()) { + RCLJava.spinOnce(node); + } + } - public static void shutdown() { - nativeShutdown(); - } + /** + * Return true if rcl is currently initialized, otherwise false. + * + * @see rcl_ok(); + * + * @return true if RCLJava hasn't been shut down, false otherwise. + */ + public static boolean ok() { + if (!RCLJava.initialized) { + throw new NotInitializedException(); + } - private static native long nativeGetZeroInitializedWaitSet(); + return RCLJava.nativeOk(); + } - private static native void nativeWaitSetInit( - long waitSetHandle, int numberOfSubscriptions, - int numberOfGuardConditions, int numberOfTimers, - int numberOfClients, int numberOfServices); + /** + * Signal global shutdown of RCLJava. + * + *

This function does not have to be called on exit, but does have to be called making a + * repeat call to RCLJava.rclJavaInit.

+ * + *

This function can only be called once after each call to RCLJava.rclJavaInit.

+ */ + public static void shutdown() { + RCLJava.logger.debug("Shutdown..."); + + if (!RCLJava.initialized) { + throw new NotInitializedException(); + } - private static native void nativeWaitSetClearSubscriptions( - long waitSetHandle); + RCLJava.nativeShutdown(); + RCLJava.initialized = false; + } - private static native void nativeWaitSetAddSubscription( - long waitSetHandle, long subscriptionHandle); + /** + * @return The identifier of the currently active RMW implementation. + */ + public static String getRMWIdentifier() { + if (!RCLJava.initialized) { + throw new NotInitializedException(); + } - private static native void nativeWait(long waitSetHandle); + return RCLJava.nativeGetRMWIdentifier(); + } - private static native Object nativeTake(long subscriptionHandle, - Class messageType); + public static List getNodeNames() { + return RCLJava.nativeGetNodeNames(); + } - private static native void nativeWaitSetClearServices(long waitSetHandle); + /** + * Get identifier of the ROS2 middleware use. + * + *

TODO rename to list of RMW available.

+ * + * @return Identifier string of ROS2 middleware. + */ + public static String getTypesupportIdentifier() { + String typesupportIdentifier = RMW_TO_TYPESUPPORT.get(RCLJava.getRMWIdentifier()); + + return typesupportIdentifier; + } - private static native void nativeWaitSetAddService(long waitSetHandle, - long serviceHandle); + /** + * Switch of ROS2 middleware implementation + * + *

TODO need to check implementation available.

+ * + * @param rmwImplementation + * @throws NoImplementationAvailableException + */ + public static void setRMWImplementation(final String rmwImplementation) + throws NoImplementationAvailableException, ImplementationAlreadyImportedException { + + synchronized(RCLJava.class) { + if (rmwImplementation != null && !rmwImplementation.isEmpty()) { + if (!rmwImplementation.equals(RCLJava.rmwImplementation)) { + + String file = "rcljavaRCLJava__" + rmwImplementation; + RCLJava.logger.debug("Load native RMW file : lib" + file + ".so"); + + try { + System.loadLibrary(file); + RCLJava.rmwImplementation = rmwImplementation; + } catch (UnsatisfiedLinkError e) { + throw new NoImplementationAvailableException(e); + } catch (Exception e) { + throw new NoImplementationAvailableException(e); + } + } else { + throw new ImplementationAlreadyImportedException(); + } + } else { + RCLJava.logger.debug("Disable RMW !"); + RCLJava.rmwImplementation = null; + } + } + } - private static native void nativeWaitSetClearClients(long waitSetHandle); + /** + * Load Native ROS library + * load from java.library.path . + * @param name Name of the library. + */ + public static void loadLibrary(final String name) { + synchronized(RCLJava.class) { + RCLJava.logger.info("Load native file : lib" + name + ".so"); + + if (!RCLJava.initialized) { + throw new NotInitializedException(); + } - private static native void nativeWaitSetAddClient(long waitSetHandle, - long clientHandle); + try { + System.loadLibrary(name); + } catch (UnsatisfiedLinkError e) { + RCLJava.logger.error("Native code library failed to load.", e); - private static native Object nativeTakeRequest( - long serviceHandle, long requestFromJavaConverterHandle, - long requestToJavaConverterHandle, Object requestMessage); + System.exit(1); + } + } + } - private static native void nativeSendServiceResponse(long serviceHandle, - Object header, long responseFromJavaConverterHandle, - long responseToJavaConverterHandle, Object responseMessage); + /** + * Load RMW. + */ + private static void autoLoadRmw() { + for (Map.Entry entry : RMW_TO_TYPESUPPORT.entrySet()) { + try { + RCLJava.logger.info("Try to load native " + entry.getKey() + "..."); + RCLJava.setRMWImplementation(entry.getKey()); + RCLJava.logger.info(entry.getKey() + " loaded !"); + break; + } catch (NoImplementationAvailableException e) { + RCLJava.logger.error(entry.getKey() + " not available ! (" + e.getMessage() + ")"); + } catch (ImplementationAlreadyImportedException e) { + RCLJava.logger.error(e.getMessage()); + } + } + } - private static native Object nativeTakeResponse( - long clientHandle, long responseFromJavaConverterHandle, - long responseToJavaConverterHandle, Object responseMessage); - public static long convertQoSProfileToHandle(final QoSProfile qosProfile) { - int history = qosProfile.getHistory().getValue(); - int depth = qosProfile.getDepth(); - int reliability = qosProfile.getReliability().getValue(); - int durability = qosProfile.getDurability().getValue(); + /** + * Convert Java QOS to JNI. + * @param qosProfile + * @return + */ + public static long convertQoSProfileToHandle(final QoSProfile qosProfile) { + int history = qosProfile.getHistory().getValue(); + int depth = qosProfile.getDepth(); + int reliability = qosProfile.getReliability().getValue(); + int durability = qosProfile.getDurability().getValue(); - return nativeConvertQoSProfileToHandle(history, depth, reliability, - durability); - } + RCLJava.logger.debug("Convert QosProfile..."); - private static native long nativeConvertQoSProfileToHandle( - int history, int depth, int reliability, int durability); + return RCLJava.nativeConvertQoSProfileToHandle(history, depth, reliability, durability); + } - public static void disposeQoSProfile(final long qosProfileHandle) { - nativeDisposeQoSProfile(qosProfileHandle); - } + /** + * Dispose JNI QosProfile. + * @param qosProfileHandle identifier + */ + public static void disposeQoSProfile(final long qosProfileHandle) { + RCLJava.nativeDisposeQoSProfile(qosProfileHandle); + } - private static native void nativeDisposeQoSProfile( - long qosProfileHandle); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/Service.java b/rcljava/src/main/java/org/ros2/rcljava/Service.java deleted file mode 100644 index c8183e13..00000000 --- a/rcljava/src/main/java/org/ros2/rcljava/Service.java +++ /dev/null @@ -1,91 +0,0 @@ -/* Copyright 2016 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; - -public class Service { - - private final long nodeHandle; - private final long serviceHandle; - private final Class serviceType; - private final String serviceName; - private final TriConsumer callback; - - private long requestFromJavaConverterHandle = 0; - private long requestToJavaConverterHandle = 0; - - private long responseFromJavaConverterHandle = 0; - private long responseToJavaConverterHandle = 0; - - private final Class requestType; - private final Class responseType; - - public Service(final long nodeHandle, final long serviceHandle, - final Class serviceType, final String serviceName, - final TriConsumer callback, - final Class requestType, final Class responseType, - final long requestFromJavaConverterHandle, - final long requestToJavaConverterHandle, - final long responseFromJavaConverterHandle, - final long responseToJavaConverterHandle) { - this.nodeHandle = nodeHandle; - this.serviceHandle = serviceHandle; - this.serviceType = serviceType; - this.serviceName = serviceName; - this.callback = callback; - this.requestType = requestType; - this.responseType = responseType; - this.requestFromJavaConverterHandle = requestFromJavaConverterHandle; - this.requestToJavaConverterHandle = requestToJavaConverterHandle; - this.responseFromJavaConverterHandle = responseFromJavaConverterHandle; - this.responseToJavaConverterHandle = responseToJavaConverterHandle; - } - - public final TriConsumer getCallback() { - return callback; - } - - public final Class getServiceType() { - return serviceType; - } - - public final long getServiceHandle() { - return serviceHandle; - } - - public final long getRequestFromJavaConverterHandle() { - return this.requestFromJavaConverterHandle; - } - - public final long getRequestToJavaConverterHandle() { - return this.requestToJavaConverterHandle; - } - - public final long getResponseFromJavaConverterHandle() { - return this.responseFromJavaConverterHandle; - } - - public final long getResponseToJavaConverterHandle() { - return this.responseToJavaConverterHandle; - } - - public final Class getRequestType() { - return this.requestType; - } - - public final Class getResponseType() { - return this.responseType; - } -} diff --git a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java b/rcljava/src/main/java/org/ros2/rcljava/Subscription.java deleted file mode 100644 index 1f593494..00000000 --- a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java +++ /dev/null @@ -1,105 +0,0 @@ -/* Copyright 2016 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; - -/** - * This class serves as a bridge between ROS2's rcl_subscription_t and RCLJava. - * A Subscription must be created via - * @{link Node#createSubscription(Class<T>, String, Consumer<T>)} - * - * @param The type of the messages that this subscription will receive. - */ -public class Subscription { - - /** - * An integer that represents a pointer to the underlying ROS2 node - * structure (rcl_node_t). - */ - private final long nodeHandle; - - /** - * An integer that represents a pointer to the underlying ROS2 subscription - * structure (rcl_subsription_t). - */ - private final long subscriptionHandle; - - /** - * The class of the messages that this subscription may receive. - */ - private final Class messageType; - - /** - * The topic to which this subscription is subscribed. - */ - private final String topic; - - /** - * The callback function that will be triggered when a new message is - * received. - */ - private final Consumer callback; - - /** - * Constructor. - * - * @param nodeHandle A pointer to the underlying ROS2 node structure that - * created this subscription, as an integer. Must not be zero. - * @param subscriptionHandle A pointer to the underlying ROS2 subscription - * structure, as an integer. Must not be zero. - * @param messageType The Class of the messages that this - * subscription will receive. We need this because of Java's type erasure, - * which doesn't allow us to use the generic parameter of - * @{link org.ros2.rcljava.Subscription} directly. - * @param topic The topic to which this subscription will be subscribed. - * @param callback The callback function that will be triggered when a new - * message is received. - */ - public Subscription(final long nodeHandle, final long subscriptionHandle, - final Class messageType, final String topic, - final Consumer callback) { - this.nodeHandle = nodeHandle; - this.subscriptionHandle = subscriptionHandle; - this.messageType = messageType; - this.topic = topic; - this.callback = callback; - } - - /** - * @return The callback function that this subscription will trigger when - * a message is received. - */ - public final Consumer getCallback() { - return callback; - } - - /** - * @return The type of the messages that this subscription may receive. - */ - public final Class getMessageType() { - return messageType; - } - - /** - * @return The pointer to the underlying ROS2 subscription structure. - */ - public final long getSubscriptionHandle() { - return subscriptionHandle; - } - - public final long getNodeHandle() { - return this.nodeHandle; - } -} diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java b/rcljava/src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java new file mode 100644 index 00000000..59d14d20 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java @@ -0,0 +1,44 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.exception; + +/** + * Raised on select_rmw_implemenation() after import_rmw_implementation() has + * been called. + * + */ +public class ImplementationAlreadyImportedException extends Exception { + + /** Serial ID */ + private static final long serialVersionUID = 4388424558654481791L; + + /** + * Constructor. + */ + public ImplementationAlreadyImportedException() { + this(null); + } + + /** + * Constructor. + * + * @param cause + */ + public ImplementationAlreadyImportedException(Throwable cause) { + super("rmw implementation already imported", cause); + } + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java b/rcljava/src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java new file mode 100644 index 00000000..b332bb5e --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java @@ -0,0 +1,36 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.exception; + +/** + * Raised when an invalid RCLJAVAImplementation is requested. + * + */ +public class InvalidRCLJAVAImplementation extends RuntimeException { + + /** Serial ID */ + private static final long serialVersionUID = -7328914635553812875L; + + /** + * Constructor. + * + * @param cause + */ + public InvalidRCLJAVAImplementation(Throwable cause) { + super("requested invalid rmw implementation", cause); + } + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java b/rcljava/src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java new file mode 100644 index 00000000..bd6dfada --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java @@ -0,0 +1,35 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.exception; + +/** + * Raised when there is no rmw implementation with a Java extension available. + * + */ +public class NoImplementationAvailableException extends Exception { + + /** Serial ID */ + private static final long serialVersionUID = -2351440132432398102L; + + /** + * Constructor. + * + * @param cause + */ + public NoImplementationAvailableException(Throwable cause) { + super("no rmw implementation with a Java extension available", cause); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/NotImplementedException.java b/rcljava/src/main/java/org/ros2/rcljava/exception/NotImplementedException.java new file mode 100644 index 00000000..2f9ca07d --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/exception/NotImplementedException.java @@ -0,0 +1,33 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.exception; + +/** + * Raised when there is no implementation. + * + */ +public class NotImplementedException extends RuntimeException { + + /** Serial ID */ + private static final long serialVersionUID = -2351440132432398102L; + + /** + * Constructor. + */ + public NotImplementedException() { + super("Not Implemented !!!"); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/NotInitializedException.java b/rcljava/src/main/java/org/ros2/rcljava/exception/NotInitializedException.java new file mode 100644 index 00000000..2fe32bab --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/exception/NotInitializedException.java @@ -0,0 +1,53 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.exception; + +/** + * Raised when the rcljava implementation is accessed before RclJava(). + * + */ +public class NotInitializedException extends RuntimeException { + + /** Serial ID */ + private static final long serialVersionUID = -5109722435632105485L; + + /** + * Constructor. + * + * @param cause + */ + public NotInitializedException() { + this("RCLJava.rclJavaInit() has not been called !", null); + } + + /** + * Constructor. + * + * @param cause + */ + public NotInitializedException(String msg) { + this(msg, null); + } + + /** + * Constructor. + * + * @param cause + */ + public NotInitializedException(String msg, Throwable cause) { + super(msg, cause); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/exception/package-info.java new file mode 100644 index 00000000..6ae5293a --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/exception/package-info.java @@ -0,0 +1,21 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 exceptions. + * + */ +package org.ros2.rcljava.exception; diff --git a/rcljava/src/main/java/org/ros2/rcljava/internal/IClient.java b/rcljava/src/main/java/org/ros2/rcljava/internal/IClient.java new file mode 100644 index 00000000..9c119a27 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/internal/IClient.java @@ -0,0 +1,35 @@ +/* Copyright 2016 Mickael Gaillard + * + * 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.internal; + +import java.util.concurrent.Future; + +public interface IClient { + + /** + * Query Service. + * + * @param Request message Type. + * @param Responce message Type. + * @param request Request of the service. + * @return Futur of Responce. + */ + Future sendRequest(U request); + + /** + * Safely destroy the underlying ROS2 Client structure. + */ + void dispose(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/internal/IGraph.java b/rcljava/src/main/java/org/ros2/rcljava/internal/IGraph.java new file mode 100644 index 00000000..36c0b1e8 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/internal/IGraph.java @@ -0,0 +1,19 @@ +/* Copyright 2016 Mickael Gaillard + * + * 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.internal; + +public interface IGraph { + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/internal/INode.java b/rcljava/src/main/java/org/ros2/rcljava/internal/INode.java new file mode 100644 index 00000000..f31f1731 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/internal/INode.java @@ -0,0 +1,213 @@ +/* Copyright 2016 Mickael Gaillard + * + * 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.internal; + +import java.util.HashMap; +import java.util.List; + +import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.node.parameter.ParameterVariant; +import org.ros2.rcljava.node.service.Client; +import org.ros2.rcljava.node.service.RMWRequestId; +import org.ros2.rcljava.node.service.Service; +import org.ros2.rcljava.node.service.TriConsumer; +import org.ros2.rcljava.node.topic.Consumer; +import org.ros2.rcljava.node.topic.Publisher; +import org.ros2.rcljava.node.topic.Subscription; + +import rcl_interfaces.msg.SetParametersResult; + +/** + * ROS2 Client API. + * + */ +public interface INode { + + /** + * Release all ressource. + */ + void dispose(); + + /** + * Get the name of the node. + * + * @return The name of the node. + */ + String getName(); + + /** + * Create and return a Publisher. + * + * @param Message definition. + * @param message Message class. + * @param topic The topic for this publisher to publish on. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return Publisher instance of the created publisher. + */ + Publisher createPublisher(Class message, String topic, QoSProfile qos); + + /** + * Create and return a Publisher. (Retro-compatibility) + * + * @param Message definition. + * @param message Message class. + * @param topic The topic for this publisher to publish on. + * @return Publisher instance of the created publisher. + */ + Publisher createPublisher(Class message, String topic); + + /** + * Create and return a Subscription. + * + * @param Message definition. + * @param message Message Class + * @param topic The topic to subscribe on. + * @param callback The user-defined callback function. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return Subscription instance of the created subscription. + */ + Subscription createSubscription(Class message, String topic, Consumer callback, QoSProfile qos); + + /** + * Create and return a Subscription. (Retro-compatibility) + * + * @param Message definition. + * @param message Message Class + * @param topic The topic to subscribe on. + * @param callback The user-defined callback function. + * @return Subscription instance of the created subscription. + */ + Subscription createSubscription(Class message, String topic, Consumer callback); + + /** + * Create and return a Client. + * + * @param Message definition. + * @param message Message Class + * @param service The service to subscribe on. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return Client instance of the service. + */ + Client createClient(Class message, String service, QoSProfile qos) throws Exception; + + /** + * Create and return a Client. (Retro-compatibility) + * + * @param Message definition. + * @param message Message Class + * @param service The service to subscribe on. + * @return Client instance of the service. + */ + Client createClient(Class message, String service) throws Exception ; + + /** + * Create and return a Service. + * + * @param Message definition. + * @param message Message Class + * @param service The service for this publisher to publish on. + * @param callback The user-defined callback function. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return Service instance of the service. + */ + Service createService(final Class serviceType, + final String serviceName, + final TriConsumer callback, + final QoSProfile qos) throws Exception; + + /** + * Create and return a Service. (Retro-compatibility) + * + * @param Message definition. + * @param message Message Class + * @param service The service for this publisher to publish on. + * @param callback The user-defined callback function. + * @return Service instance of the service. + */ + Service createService(final Class serviceType, + final String serviceName, + final TriConsumer callback) throws Exception; + + List setParameters(final List> parameters); + + SetParametersResult setParametersAtomically(final List> parameters); + + List> getParameters(final List names); + + ParameterVariant getParameter(final String name); + + HashMap getTopicNamesAndTypes(); + + int countPublishers(final String topic); + + int countSubscribers(final String topic); + + /** + * Return the rcl_node_t node handle (non-const version). + * @return + */ + long getNodeHandle(); + + /** + * Notify threads waiting on graph changes. + * + * Affects threads waiting on the notify guard condition, see: + * get_notify_guard_condition(), as well as the threads waiting on graph + * changes using a graph Event, see: wait_for_graph_change(). + * + * This is typically only used by the rclcpp::graph_listener::GraphListener. + * + * \throws RCLBaseError (a child of that exception) when an rcl error occurs + */ + void notifyGraphChange(); + + /** Notify any and all blocking node actions that shutdown has occurred. */ + void notifyShutdown(); + + /** + * Return a graph event, which will be set anytime a graph change occurs. + * + * The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the load just let it go + * out of scope. + */ + Object getGraphEvent(); + + /** + * Wait for a graph event to occur by waiting on an Event to become set. + * + * The given Event must be acquire through the get_graph_event() method. + * + * \throws InvalidEventError if the given event is nullptr + * \throws EventNotRegisteredError if the given event was not acquired with + * get_graph_event(). + */ + void waitForGraphChange(Object event, int timeout); + + /** + * This is typically only used by the rclcpp::graph_listener::GraphListener. + * Return the number of on loan graph events, see get_graph_event(). + */ + int countGraphUsers(); + + /** + * Register the callback for parameter changes. + * Repeated invocations of this function will overwrite previous callbacks + * + * @param User defined callback function, It is expected to atomically set parameters. + */ + void registerParamChangeCallback(Consumer callback); + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/internal/IPublisher.java b/rcljava/src/main/java/org/ros2/rcljava/internal/IPublisher.java new file mode 100644 index 00000000..84ec67c5 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/internal/IPublisher.java @@ -0,0 +1,30 @@ +/* Copyright 2016 Mickael Gaillard + * + * 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.internal; + +public interface IPublisher { + + /** + * Publish a message. + * + * @param message An instance of the <T> parameter. + */ + void publish(final T message); + + /** + * Safely destroy the underlying ROS2 publisher structure. + */ + void dispose(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/internal/IService.java b/rcljava/src/main/java/org/ros2/rcljava/internal/IService.java new file mode 100644 index 00000000..4e4200a0 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/internal/IService.java @@ -0,0 +1,23 @@ +/* Copyright 2016 Mickael Gaillard + * + * 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.internal; + +public interface IService { + + /** + * Safely destroy the underlying ROS2 Service structure. + */ + void dispose(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/internal/ISubscription.java b/rcljava/src/main/java/org/ros2/rcljava/internal/ISubscription.java new file mode 100644 index 00000000..393051e1 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/internal/ISubscription.java @@ -0,0 +1,23 @@ +/* Copyright 2016 Mickael Gaillard + * + * 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.internal; + +public interface ISubscription { + + /** + * Safely destroy the underlying ROS2 subscriber structure. + */ + void dispose(); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/internal/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/internal/package-info.java new file mode 100644 index 00000000..0da9acac --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/internal/package-info.java @@ -0,0 +1,19 @@ +/* Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 Interface of RCL + */ +package org.ros2.rcljava.internal; diff --git a/rcljava/src/main/java/org/ros2/rcljava/namespace/GraphName.java b/rcljava/src/main/java/org/ros2/rcljava/namespace/GraphName.java new file mode 100644 index 00000000..9acbc582 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/namespace/GraphName.java @@ -0,0 +1,182 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.namespace; + +import java.util.List; +import java.util.Queue; +import java.util.concurrent.LinkedBlockingQueue; + +import org.ros2.rcljava.internal.IGraph; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.service.Service; +import org.ros2.rcljava.node.topic.Topics; + +public class GraphName implements IGraph { + + /** + * All the @{link Node}s that have been created. + */ + private static Queue nodes = new LinkedBlockingQueue(); + + public static void addNode(Node node) { +// if (initialized) { + GraphName.nodes.add(node); +// } + } + + public static void removeNode(Node node) { +// if (initialized) { + GraphName.nodes.remove(node); +// } + } + + public static void dispose() { + for (Node node : GraphName.nodes) { + node.dispose(); + } + } + + private static String removeSheme(final String url) { + String topicName = url; + boolean result = topicName != null; + + if (result) { + topicName = topicName.replaceAll(Topics.SCHEME, ""); + topicName = topicName.replaceAll(Service.SCHEME, ""); + } + + return topicName; + } + + /** + * + * + * + * + * + * + * + *
Input Name Node: my_node NS: none Node: my_node NS: /my_ns
ping /ping /my_ns/ping
/ping /ping /ping
~ /my_node /my_ns/my_node
~/ping /my_node/ping /my_ns/my_node/ping
+ * + * @param node + * @param name + * @param options + * @return + */ + public static String getFullName(final Node node, final String name, List options) { + String result = name; + String nodeName = node.getName(); + String ns = node.getNameSpace(); + + // Manage Name Space. + if (ns == null || ns.length() == 0) { + ns = ""; + } else if (!ns.startsWith("/")){ + ns = String.format("/%s", ns); + } + + // Absolute case + if (name.startsWith("/")) { + result = name; + } else + + // Relative case + if (name.startsWith("~/")) { + String relPath = name.replaceFirst("~/", ""); + result = String.format("%s/%s/%s", ns, nodeName, relPath); + } else + + // Node case + if (name.startsWith("~")) { + result = String.format("%s/%s", ns, nodeName); + } else + + // Name space path cas + { + result = String.format("%s/%s", ns, name); + } + + return result; + } + + /** + * Check if name is valid. + * + * For testing : https://regex101.com/r/b3R65h/1 + * @param topicName + * @return + */ + public static boolean isValidTopic(final String name) { + // Delete sample Regex : https://regex101.com/delete/7qMZiThwlEla6eAKIIee3251 + boolean result = name != null; + + if (result) { + String topicName = GraphName.removeSheme(name); + + result = + topicName != null && + topicName.length() > 0 && + topicName.matches("^(?!.*(//|__|_/))[~A-Za-z/_{}][_A-Za-z0-9/{}]*$") && + !topicName.endsWith("_") && + !topicName.endsWith("/") + ; + + if (result && topicName.startsWith("~") && topicName.length() > 1) { + result = topicName.startsWith("~/"); + } + } + + return result; + } + + /** + * Check if substitution is valid. + * + * @param substitution + * @return + */ + public static boolean isValidSubstitution(final String substitution) { + boolean result = + substitution != null && + substitution.length() > 0 && + substitution.matches("^[_A-Za-z_/][_A-Za-z0-9/]*$") && + !substitution.endsWith("_"); + + return result; + } + + /** + * Chack if Full Qualify Name is valid. + * + * @param fqn + * @return + */ + public static boolean isValideFQDN(final String fqn) { + boolean result = fqn != null; + + if (result) { + String topicName = GraphName.removeSheme(fqn); + + result = + GraphName.isValidTopic(topicName) && + topicName.startsWith("/") && + !topicName.contains("~") && + !topicName.contains("{}"); + } + + return result; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/namespace/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/namespace/package-info.java new file mode 100644 index 00000000..3837487e --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/namespace/package-info.java @@ -0,0 +1,21 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 Graph Stack. + * + */ +package org.ros2.rcljava.namespace; diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java new file mode 100644 index 00000000..6c98db39 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -0,0 +1,780 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node; + +import java.lang.ref.WeakReference; +import java.lang.reflect.Method; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; +import java.util.Map.Entry; +import java.util.Queue; +import java.util.concurrent.LinkedBlockingQueue; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.Log; +import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.exception.NotImplementedException; +import org.ros2.rcljava.internal.INode; +import org.ros2.rcljava.namespace.GraphName; +import org.ros2.rcljava.node.parameter.ParameterConsumer; +import org.ros2.rcljava.node.parameter.ParameterService; +import org.ros2.rcljava.node.parameter.ParameterVariant; +import org.ros2.rcljava.node.service.Client; +import org.ros2.rcljava.node.service.RMWRequestId; +import org.ros2.rcljava.node.service.Service; +import org.ros2.rcljava.node.service.TriConsumer; +import org.ros2.rcljava.node.topic.Consumer; +import org.ros2.rcljava.node.topic.Publisher; +import org.ros2.rcljava.node.topic.Subscription; + +import builtin_interfaces.msg.Time; +import rcl_interfaces.msg.Parameter; +import rcl_interfaces.msg.ParameterEvent; +import rcl_interfaces.msg.ParameterType; +import rcl_interfaces.msg.SetParametersResult; + +/** + * This class serves as a bridge between ROS2's rcl_node_t and RCLJava. + * A Node must be created via @{link RCLJava#createNode(String)} + * + */ +public class Node implements INode { + + private static final Logger logger = LoggerFactory.getLogger(Node.class); + + // Loading JNI library. + static { + RCLJava.loadLibrary("rcljavanode_Node__" + RCLJava.getRMWIdentifier()); + } + + /** Name of the node */ + private String name; + + /** Name space of the node. */ + private String nameSpace; + + /** + * An integer that represents a pointer to the underlying ROS2 node + * structure (rcl_node_t). + */ + private final long nodeHandle; + + /** + * All the @{link Subscription}s that have been created through this instance. + */ + private final Queue> subscriptions; + + /** + * All the @{link Publisher}s that have been created through this instance. + */ + private final Queue> publishers; + + /** + * All the @{link Service}s that have been created through this instance. + */ + private final Queue> services; + + /** + * All the @{link Client}s that have been created through this instance. + */ + private final Queue> clients; + + /** + * List of parameters + */ + private final HashMap> parameters; + + /** + * Parameter service. + */ + private final ParameterService parameterService; + private ParameterConsumer parameterCallback ; + + public void onParamChange(ParameterConsumer parameterCallback) { + this.parameterCallback = parameterCallback; + } + + /** + * Log to ROS2. + */ + private final Log logRos; + + // Native call. + /** + * Create a ROS2 publisher (rcl_publisher_t) and return a pointer to + * it as an integer. + * + * @param The type of the messages that will be published by the + * created @{link Publisher}. + * @param nodeHandle A pointer to the underlying ROS2 node structure. + * @param messageType The class of the messages that will be published by the + * created @{link Publisher}. + * @param topic The topic to which the created @{link Publisher} will + * publish messages. + * @param qosProfileHandle A pointer to the underlying ROS2 QoS profile + * structure. + * @return A pointer to the underlying ROS2 publisher structure. + */ + private static native long nativeCreatePublisherHandle( + long nodeHandle, Class messageType, String topic, long qosProfileHandle); + + /** + * Create a ROS2 subscription (rcl_subscription_t) and return a pointer to + * it as an integer. + * + * @param The type of the messages that will be received by the + * created @{link Subscription}. + * @param nodeHandle A pointer to the underlying ROS2 node structure. + * @param messageType The class of the messages that will be received by the + * created @{link Subscription}. + * @param topic The topic from which the created @{link Subscription} will + * receive messages. + * @param qosProfileHandle A pointer to the underlying ROS2 QoS profile + * structure. + * @return A pointer to the underlying ROS2 subscription structure. + */ + private static native long nativeCreateSubscriptionHandle( + long nodeHandle, Class messageType, String topic, long qosProfileHandle); + + private static native long nativeCreateClientHandle( + long nodeHandle, Class cls, String serviceName, long qosProfileHandle); + + private static native long nativeCreateServiceHandle( + long nodeHandle, Class cls, String serviceName, long qosProfileHandle); + + private static native void nativeDispose(long nodeHandle); + + private static native String nativeGetName(long nodeHandle); + + private static native int nativeCountPublishers(long nodeHandle, String topic); + + private static native int nativeCountSubscribers(long nodeHandle, String topic); + + private static native HashMap getListTopics(long nodeHandle); + +// private static native ; //rcl_service_server_is_available + + /** + * Constructor. + * + * @param nodeHandle A pointer to the underlying ROS2 node structure. Must not + * be zero. + */ + public Node(final long nodeHandle,final String ns, final String nodeName) { + + if (nodeHandle==0) throw new NullPointerException("Node Handle is not define !"); + if (nodeName==null || nodeName.length() == 0) throw new NullPointerException("Node name is needed !"); + + this.nameSpace = ns; + this.name = nodeName; + this.nodeHandle = nodeHandle; + this.subscriptions = new LinkedBlockingQueue>(); + this.publishers = new LinkedBlockingQueue>(); + this.clients = new LinkedBlockingQueue>(); + this.services = new LinkedBlockingQueue>(); + this.parameters = new HashMap>(); + + Node.logger.debug("Init Node stack : " + this.name); + + GraphName.addNode(this); + this.parameterService = new ParameterService(this); + this.logRos = new Log(this); + } + + /** + * Safely destroy the underlying ROS2 node structure. + */ + @Override + public void dispose() { + Node.logger.debug("Destroy Node stack : " + this.name); + this.parameterService.dispose(); + + Queue> tmpClients = new LinkedBlockingQueue>(this.clients); + for (Client client : tmpClients) { + client.dispose(); + } + + Queue> tmpPublishers = new LinkedBlockingQueue>(this.publishers); + for (Publisher publisher : tmpPublishers) { + publisher.dispose(); + } + + Queue> tmpServices = new LinkedBlockingQueue>(this.services); + for (Service service : tmpServices) { + service.dispose(); + } + + Queue> tmpSubscribers = new LinkedBlockingQueue>(this.subscriptions); + for (Subscription subscriber : tmpSubscribers) { + subscriber.dispose(); + } + + Node.nativeDispose(this.nodeHandle); + GraphName.removeNode(this); + } + + /** + * Get the name of the node. + * + * @return The name of the node. + */ + @Override + public String getName() { + String name = Node.nativeGetName(this.nodeHandle); + + if (!this.name.equals(name)) { + Node.logger.debug("Node name has changed ! from " + this.name + " to " + name); + this.name = name; + } + + return name; + } + + /** + * Create a Publisher<T>. + * + * @param The type of the messages that will be published by the + * created @{link Publisher}. + * @param messageType The class of the messages that will be published by the + * created @{link Publisher}. + * @param topic The topic to which the created @{link Publisher} will + * publish messages. + * + * @return A @{link Publisher} that represents the underlying ROS2 publisher + * structure. + */ + @Override + public Publisher createPublisher( + final Class messageType, + final String topic, + final QoSProfile qosProfile) { + + if (messageType==null) throw new NullPointerException("Message Type can't be null."); + if (topic==null) throw new NullPointerException("Topic can't be null."); + if (qosProfile==null) throw new NullPointerException("QOS can't be null;"); + + String fqnTopic = GraphName.getFullName(this, topic, null); + Node.logger.debug("Create Publisher : " + fqnTopic); + Publisher publisher = null; + + if (GraphName.isValidTopic(fqnTopic)) { + long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); + long publisherHandle = Node.nativeCreatePublisherHandle(this.nodeHandle, messageType, fqnTopic, qosProfileHandle); + RCLJava.disposeQoSProfile(qosProfileHandle); + + publisher = new Publisher(this, publisherHandle, messageType, topic, qosProfile); + } + + return publisher; + } + + /** + * Create and return a Publisher. (Retro-compatibility) + * + * @param Message definition. + * @param messageType Message class. + * @param topic The topic for this publisher to publish on. + * @return Publisher instance of the created publisher. + */ + @Override + public Publisher createPublisher( + final Class messageType, + final String topic) { + return this.createPublisher(messageType, topic, QoSProfile.DEFAULT); + } + + /** + * Create a Subscription<T>. + * + * @param The type of the messages that will be received by the + * created @{link Subscription}. + * @param messageType The class of the messages that will be received by the + * created @{link Subscription}. + * @param topic The topic from which the created @{link Subscription} will + * receive messages. + * @param callback The callback function that will be triggered when a + * message is received by the @{link Subscription}. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return A @{link Subscription} that represents the underlying ROS2 + * subscription structure. + */ + @Override + public Subscription createSubscription( + final Class messageType, + final String topic, + final Consumer callback, + final QoSProfile qosProfile) { + + if (messageType==null) throw new NullPointerException("Message Type can't be null."); + if (topic==null) throw new NullPointerException("Topic can't be null."); + if (callback==null) throw new NullPointerException("Callback can't be null;"); + if (qosProfile==null) throw new NullPointerException("QOS can't be null;"); + + String fqnTopic = GraphName.getFullName(this, topic, null); + Node.logger.debug("Create Subscription : " + fqnTopic); + Subscription subscription = null; + + if (GraphName.isValidTopic(fqnTopic)) { + long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); + long subscriptionHandle = Node.nativeCreateSubscriptionHandle(this.nodeHandle, messageType, fqnTopic, qosProfileHandle); + RCLJava.disposeQoSProfile(qosProfileHandle); + + subscription = new Subscription( + this, + subscriptionHandle, + messageType, + topic, + callback, + qosProfile); + } + + return subscription; + } + + /** + * Create and return a Subscription. (Retro-compatibility) + * + * @param Message definition. + * @param messageType Message Class + * @param topic The topic to subscribe on. + * @param callback The user-defined callback function. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return Subscription instance of the created subscription. + */ + @Override + public Subscription createSubscription( + final Class messageType, + final String topic, + final Consumer callback) { + return this.createSubscription(messageType, topic, callback, QoSProfile.DEFAULT); + } + + /** + * Create and return a Client. + * + * @param Message definition. + * @param serviceType Service Class + * @param serviceName The service to subscribe on. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return Client instance of the service. + * @throws SecurityException + * @throws NoSuchFieldException + * @throws IllegalAccessException + * @throws IllegalArgumentException + * @throws NoSuchMethodException + */ + @Override + public Client createClient( + final Class serviceType, + final String serviceName, + final QoSProfile qosProfile) throws Exception { + + + if (serviceType==null) throw new NullPointerException("Service Type can't be null."); + if (serviceName==null) throw new NullPointerException("Service name can't be null."); + if (qosProfile==null) throw new NullPointerException("QOS can't be null;"); + + String fqnService = GraphName.getFullName(this, serviceName, null); + Node.logger.debug("Create Client : " + fqnService); + Client client = null; + + if (GraphName.isValidTopic(fqnService)) { + Class requestType = (Class)serviceType.getField("RequestType").get(null); + + Method requestFromJavaConverterMethod = requestType.getDeclaredMethod("getFromJavaConverter", (Class []) null); + long requestFromJavaConverterHandle = (Long)requestFromJavaConverterMethod.invoke(null, (Object []) null); + + Method requestToJavaConverterMethod = requestType.getDeclaredMethod("getToJavaConverter", (Class []) null); + long requestToJavaConverterHandle = (Long)requestToJavaConverterMethod.invoke(null, (Object []) null); + + Class responseType = (Class)serviceType.getField("ResponseType").get(null); + + Method responseFromJavaConverterMethod = responseType.getDeclaredMethod("getFromJavaConverter", (Class []) null); + long responseFromJavaConverterHandle = (Long)responseFromJavaConverterMethod.invoke(null, (Object []) null); + + Method responseToJavaConverterMethod = responseType.getDeclaredMethod("getToJavaConverter", (Class []) null); + long responseToJavaConverterHandle = (Long)responseToJavaConverterMethod.invoke(null, (Object []) null); + + long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); + long clientHandle = Node.nativeCreateClientHandle( + this.nodeHandle, + serviceType, + fqnService, + qosProfileHandle); + RCLJava.disposeQoSProfile(qosProfileHandle); + + client = new Client( + new WeakReference(this), + clientHandle, + serviceType, + serviceName, + requestType, + responseType, + requestFromJavaConverterHandle, + requestToJavaConverterHandle, + responseFromJavaConverterHandle, + responseToJavaConverterHandle); + } + + return client; + } + + /** + * Create and return a Client. (Retro-compatibility) + * + * @param Message definition. + * @param message Message Class + * @param service The service to subscribe on. + * @return Client instance of the service. + * @throws Exception + */ + @Override + public Client createClient( + final Class serviceType, + final String serviceName) throws Exception { + return this.createClient(serviceType, serviceName, QoSProfile.SERVICES_DEFAULT); + } + + /** + * Create and return a Service. + * + * @param Message definition. + * @param serviceType Service Class + * @param serviceName The service for this publisher to publish on. + * @param callback The user-defined callback function. + * @param qos The quality of service profile to pass on to the rmw implementation. + * @return Service instance of the service. + */ + @Override + public Service createService( + final Class serviceType, + final String serviceName, + final TriConsumer callback, + final QoSProfile qosProfile + ) throws Exception { + + if (serviceType==null) throw new NullPointerException("Service Type can't be null."); + if (serviceName==null) throw new NullPointerException("Service name can't be null."); + if (callback==null) throw new NullPointerException("Callback can't be null;"); + if (qosProfile==null) throw new NullPointerException("QOS can't be null;"); + + String fqnService = GraphName.getFullName(this, serviceName, null); + Node.logger.debug("Create Service : " + fqnService); + Service service = null; + + if (GraphName.isValidTopic(fqnService)) { +// long serviceHandle = Node.nativeCreateServiceHandle(this.nodeHandle, message, service, qos); +// Service srv = new Service(this.nodeHandle, serviceHandle, service); + + Class requestType = (Class)serviceType.getField("RequestType").get(null); + + Method requestFromJavaConverterMethod = requestType.getDeclaredMethod("getFromJavaConverter", (Class []) null); + long requestFromJavaConverterHandle = (Long)requestFromJavaConverterMethod.invoke(null, (Object []) null); + + Method requestToJavaConverterMethod = requestType.getDeclaredMethod("getToJavaConverter", (Class []) null); + long requestToJavaConverterHandle = (Long)requestToJavaConverterMethod.invoke(null, (Object []) null); + + Class responseType = (Class)serviceType.getField("ResponseType").get(null); + + Method responseFromJavaConverterMethod = responseType.getDeclaredMethod("getFromJavaConverter", (Class []) null); + long responseFromJavaConverterHandle = (Long)responseFromJavaConverterMethod.invoke(null, (Object []) null); + + Method responseToJavaConverterMethod = responseType.getDeclaredMethod("getToJavaConverter", (Class []) null); + long responseToJavaConverterHandle = (Long)responseToJavaConverterMethod.invoke(null, (Object []) null); + + long qosProfileHandle = RCLJava.convertQoSProfileToHandle(qosProfile); + long serviceHandle = Node.nativeCreateServiceHandle(this.nodeHandle, serviceType, serviceName, qosProfileHandle); + RCLJava.disposeQoSProfile(qosProfileHandle); + + service = new Service(this, + serviceHandle, + serviceType, + fqnService, + callback, + requestType, + responseType, + requestFromJavaConverterHandle, + requestToJavaConverterHandle, + responseFromJavaConverterHandle, + responseToJavaConverterHandle); + } + + return service; + } + + /** + * Create and return a Service. (Retro-compatibility) + * + * @param Message definition. + * @param message Message Class + * @param service The service for this publisher to publish on. + * @param callback The user-defined callback function. + * @return Service instance of the service. + */ + @Override + public Service createService( + final Class serviceType, + final String serviceName, + final TriConsumer callback) throws Exception { + + return this.createService(serviceType, serviceName, callback, QoSProfile.SERVICES_DEFAULT); + } + + @Override + public List setParameters(final List> parameters) { + List results = new ArrayList(); + + for (ParameterVariant parameterVariantRequest : parameters) { + SetParametersResult result = this.setParametersAtomically(new ArrayList>(Arrays.asList(parameterVariantRequest))); + results.add(result); + } + + return results; + } + + @Override + public SetParametersResult setParametersAtomically(final List> parameters) { + ParameterEvent parameter_event = new ParameterEvent(); + + // TODO (jacquelinekay) Check handle parameter constraints + SetParametersResult result = new SetParametersResult(); + + if (this.parameterCallback != null) { + result = this.parameterCallback.onParamChange(parameters); + } else { + result.setSuccessful(true); + } + + if (result.getSuccessful()){ + for (ParameterVariant paramVarReq : parameters) { + Parameter parameter = paramVarReq.toParameter(); + + if (!this.parameters.containsKey(paramVarReq.getName())) { + if (parameter.getValue().getType() != ParameterType.PARAMETER_NOT_SET) { + parameter_event.getNewParameters().add(parameter); + } + } else { + if (parameter.getValue().getType() != ParameterType.PARAMETER_NOT_SET) { + parameter_event.getChangedParameters().add(parameter); + } else { + parameter_event.getDeletedParameters().add(parameter); + } + } + this.parameters.put(paramVarReq.getName(), paramVarReq); + } + } + + if (this.parameterService != null) { + this.parameterService.notifyAddEvent(parameter_event); + } + + return result; + } + + @Override + public List> getParameters(final List names) { + List> result = new ArrayList>(); + + for (String name : names) { + ParameterVariant param = this.getParameter(name); + if (param != null) { + result.add(param); + } + } + + return result; + } + + @Override + public ParameterVariant getParameter(final String name) { + ParameterVariant result = null; + + if (this.parameters.containsKey(name)) { + result = this.parameters.get(name); + } + + return result; + } + + public List getParametersNames() { + return new ArrayList(this.parameters.keySet()); + } + + public List getParametersTypes(List names) { + List result = new ArrayList(); + + for (String name : names) { + if (this.parameters.containsKey(name)) { + result.add(this.parameters.get(name).toParameterValue().getType()); + } + } + + return result; + } + + @Override + public HashMap getTopicNamesAndTypes() { + HashMap topics = Node.getListTopics(this.nodeHandle); + + for (Entry entry : topics.entrySet()) { + Node.logger.debug("\t - Topics: " + entry.getKey() + "\t Value: " + entry.getValue()); + } + + return topics; + } + + @Override + public int countPublishers(final String topic) { + return Node.nativeCountPublishers(this.nodeHandle, topic); + } + + @Override + public int countSubscribers(final String topic) { + return Node.nativeCountSubscribers(this.nodeHandle, topic); + } + + /** + * Return the rcl_node_t node handle (non-const version). + * @return + */ + @Override + public final long getNodeHandle() { + return this.nodeHandle; + } + + /** + * Notify threads waiting on graph changes. + * + * Affects threads waiting on the notify guard condition, see: + * get_notify_guard_condition(), as well as the threads waiting on graph + * changes using a graph Event, see: wait_for_graph_change(). + * + * This is typically only used by the rclcpp::graph_listener::GraphListener. + * + * @throws RCLBaseError (a child of that exception) when an rcl error occurs + */ + @Override + public void notifyGraphChange() { + //TODO + throw new NotImplementedException(); + } + + /** Notify any and all blocking node actions that shutdown has occurred. */ + @Override + public void notifyShutdown() { + //TODO + throw new NotImplementedException(); + } + + /** + * Return a graph event, which will be set anytime a graph change occurs. + * + * The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the load just let it go + * out of scope. + */ + @Override + public Object getGraphEvent() { + //TODO + throw new NotImplementedException(); +// return null; + } + + /** + * Wait for a graph event to occur by waiting on an Event to become set. + * + * The given Event must be acquire through the get_graph_event() method. + * + * @throws InvalidEventError if the given event is nullptr + * @throws EventNotRegisteredError if the given event was not acquired with get_graph_event(). + */ + @Override + public void waitForGraphChange(Object event, int timeout) { + //TODO + throw new NotImplementedException(); + } + + /** + * This is typically only used by the rclcpp::graph_listener::GraphListener. + * @return the number of on loan graph events, see get_graph_event(). + */ + @Override + public int countGraphUsers() { + //TODO + throw new NotImplementedException(); +// return 0; + } + + /** + * Register the callback for parameter changes. + * Repeated invocations of this function will overwrite previous callbacks + * + * @param User defined callback function, It is expected to atomically set parameters. + */ + @Override + public void registerParamChangeCallback(Consumer callback) { + //TODO + throw new NotImplementedException(); + } + + /** + * @return All the @{link Subscription}s that were created by this instance. + */ + public Queue> getSubscriptions() { + return this.subscriptions; + } + + /** + * @return All the @{link Publisher}s that were created by this instance. + */ + public final Queue> getPublishers() { + return this.publishers; + } + + /** + * Get list of Clients. + * @return ArrayList of Clients + */ + public final Queue> getClients() { + return this.clients; + } + + /** + * Get list of Services. + * @return ArrayList of Services + */ + public final Queue> getServices() { + return this.services; + } + + public Time getCurrentTime() { + //TODO (theos) + return null; + } + + public Log getLog() { + return this.logRos; + } + + public String getNameSpace() { + return this.nameSpace; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/node/package-info.java new file mode 100644 index 00000000..1789d219 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/package-info.java @@ -0,0 +1,20 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 Node Stack. + */ +package org.ros2.rcljava.node; diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterConsumer.java b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterConsumer.java new file mode 100644 index 00000000..f0503605 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterConsumer.java @@ -0,0 +1,32 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.parameter; + +import java.util.List; + +import rcl_interfaces.msg.SetParametersResult; + +/** + * Parameter Consumer. + * + */ +public interface ParameterConsumer { + /** + * + * @param event + */ + SetParametersResult onParamChange(final List> config); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterEventConsumer.java b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterEventConsumer.java new file mode 100644 index 00000000..82c7f239 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterEventConsumer.java @@ -0,0 +1,28 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.parameter; + +/** + * Parameter Consumer. + * + */ +public interface ParameterEventConsumer { + /** + * + * @param event + */ + void onEvent(final rcl_interfaces.msg.ParameterEvent event); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterService.java b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterService.java new file mode 100644 index 00000000..0ae72b7c --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterService.java @@ -0,0 +1,284 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.parameter; + +import java.util.ArrayList; +import java.util.List; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.namespace.GraphName; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.service.RMWRequestId; +import org.ros2.rcljava.node.service.Service; +import org.ros2.rcljava.node.service.TriConsumer; +import org.ros2.rcljava.node.topic.Publisher; +import org.ros2.rcljava.node.topic.Topics; + +import rcl_interfaces.msg.ListParametersResult; +import rcl_interfaces.msg.Parameter; +import rcl_interfaces.msg.ParameterDescriptor; +import rcl_interfaces.msg.ParameterValue; +import rcl_interfaces.msg.SetParametersResult; +import rcl_interfaces.msg.ParameterEvent; + +import rcl_interfaces.srv.DescribeParameters; +import rcl_interfaces.srv.DescribeParameters_Request; +import rcl_interfaces.srv.DescribeParameters_Response; +import rcl_interfaces.srv.GetParameterTypes; +import rcl_interfaces.srv.GetParameterTypes_Request; +import rcl_interfaces.srv.GetParameterTypes_Response; +import rcl_interfaces.srv.GetParameters; +import rcl_interfaces.srv.GetParameters_Request; +import rcl_interfaces.srv.GetParameters_Response; +import rcl_interfaces.srv.ListParameters; +import rcl_interfaces.srv.ListParameters_Request; +import rcl_interfaces.srv.ListParameters_Response; +import rcl_interfaces.srv.SetParameters; +import rcl_interfaces.srv.SetParameters_Request; +import rcl_interfaces.srv.SetParameters_Response; +import rcl_interfaces.srv.SetParametersAtomically; +import rcl_interfaces.srv.SetParametersAtomically_Request; +import rcl_interfaces.srv.SetParametersAtomically_Response; + +/** + * Parameter Variant. + * + */ +public class ParameterService { + private static final Logger logger = LoggerFactory.getLogger(ParameterService.class); + + protected static final String TOPIC_GETPARAMETERS = "~/_get_parameters"; + protected static final String TOPIC_GETPARAMETERTYPES = "~/_get_parameter_types"; + protected static final String TOPIC_SETPARAMETERS = "~/_set_parameters"; + protected static final String TOPIC_SETPARAMETERSATOMICALLY = "~/_set_parameters_atomically"; + protected static final String TOPIC_DESCRIBEPARAMETERS = "~/_describe_parameters"; + protected static final String TOPIC_LISTPARAMETERS = "~/_list_parameters"; + + + private final Node ownerNode; + + private Service getParametersService; + private Service getParameterTypesService; + private Service setParametersService; + private Service setParametersAtomicallyService; + private Service describeParametersService; + private Service listParametersService; + private Publisher eventparameterPublisher; + + public ParameterService(final Node node) { + this(node, QoSProfile.PARAMETER); + } + + public ParameterService(final Node node, final QoSProfile profileParameter) { + this.ownerNode = node; + + try { + logger.debug("Create Parameters stack " + node.getName()); + + final String fqnGetParameter = GraphName.getFullName(node, TOPIC_GETPARAMETERS, null); + if (GraphName.isValidTopic(fqnGetParameter)) { + this.getParametersService = node.createService( + GetParameters.class, + fqnGetParameter, + new TriConsumer() { + + @Override + public void accept( + final RMWRequestId header, + final GetParameters_Request request, + final GetParameters_Response response) { + logger.debug("Replies to get Parameters."); + List paramsResult = new ArrayList(); + + List> paramsCurrent = node.getParameters(request.getNames()); + for (ParameterVariant parameterVariant : paramsCurrent) { + paramsResult.add(parameterVariant.toParameterValue()); + } + + response.setValues(paramsResult); + } + }, + profileParameter); + } + + final String fqnGetParametertypes = GraphName.getFullName(node, TOPIC_GETPARAMETERTYPES, null); + if (GraphName.isValidTopic(fqnGetParametertypes)) { + this.getParameterTypesService = node.createService( + GetParameterTypes.class, + fqnGetParametertypes, + new TriConsumer() { + + @Override + public void accept( + final RMWRequestId header, + final GetParameterTypes_Request request, + final GetParameterTypes_Response response) { + logger.debug("Replies to get Parameter Types !"); + + response.setTypes(node.getParametersTypes(request.getNames())); + } + }, + profileParameter); + } + + final String fqnSetParameters = GraphName.getFullName(node, TOPIC_SETPARAMETERS, null); + if (GraphName.isValidTopic(fqnSetParameters)) { + this.setParametersService = node.createService( + SetParameters.class, + fqnSetParameters, + new TriConsumer() { + + @Override + public void accept( + final RMWRequestId header, + final SetParameters_Request request, + final SetParameters_Response response) { + + logger.debug("Replies to set Parameters."); + List> parameterVariants = new ArrayList>(); + for (Parameter parameterVariant : request.getParameters()) { + parameterVariants.add(ParameterVariant.fromParameter(parameterVariant)); + } + List result = node.setParameters(parameterVariants); + response.setResults(result); + } + }, + profileParameter); + } + + final String fqnSetParametersAtomically = GraphName.getFullName(node, TOPIC_SETPARAMETERSATOMICALLY, null); + if (GraphName.isValidTopic(fqnSetParametersAtomically)) { + this.setParametersAtomicallyService = node.createService( + SetParametersAtomically.class, + fqnSetParametersAtomically, + new TriConsumer() { + + @Override + public void accept( + final RMWRequestId header, + final SetParametersAtomically_Request request, + final SetParametersAtomically_Response response) { + + logger.debug("Replies to set Parameters Atomically."); + List> parameterVariants = new ArrayList>(); + for (Parameter parameterVariant : request.getParameters()) { + parameterVariants.add(ParameterVariant.fromParameter(parameterVariant)); + } + response.setResult(node.setParametersAtomically(parameterVariants)); + } + }, + profileParameter); + } + + final String fqnDescribeParameters = GraphName.getFullName(node, TOPIC_DESCRIBEPARAMETERS, null); + if (GraphName.isValidTopic(fqnDescribeParameters)) { + this.describeParametersService = node.createService( + DescribeParameters.class, + fqnDescribeParameters, + new TriConsumer() { + + @Override + public void accept( + final RMWRequestId header, + final DescribeParameters_Request request, + final DescribeParameters_Response response) { + + logger.debug("Replies to describe Parameters. ! NOT IMPLEMENTED !"); + + List listDescritiorResult = new ArrayList(); + + ParameterDescriptor descriptor = new ParameterDescriptor(); + // descriptor.setName(arg0); + // descriptor.setType(arg0); + listDescritiorResult.add(descriptor); + + response.setDescriptors(listDescritiorResult); + } + }, + profileParameter); + } + + final String fqnListParameters = GraphName.getFullName(node, TOPIC_LISTPARAMETERS, null); + if (GraphName.isValidTopic(fqnListParameters)) { + this.listParametersService = node.createService( + ListParameters.class, + fqnListParameters, + new TriConsumer() { + + @Override + public void accept( + final RMWRequestId header, + final ListParameters_Request request, + final ListParameters_Response response) { + + logger.debug("Replies to list of Parameters."); + ListParametersResult listParamResult = new ListParametersResult(); + listParamResult.setNames(node.getParametersNames()); + response.setResult(listParamResult); + + } + }, + profileParameter); + } + + final String fqnParametersEvent = GraphName.getFullName(node, Topics.PARAM_EVENT, null); + if (GraphName.isValidTopic(fqnListParameters)) { + this.eventparameterPublisher = node.createPublisher(ParameterEvent.class, fqnParametersEvent, QoSProfile.PARAMETER_EVENTS); + } + + } catch (Exception e) { + e.printStackTrace(); + } + } + + public void notifyAddEvent(final ParameterEvent param) { + if (this.eventparameterPublisher != null) { + this.eventparameterPublisher.publish(param); + } + } + + /** + * Safely destroy the underlying ROS2 subscriber structure. + */ + public void dispose() { + logger.debug("Destroy parameter client : " + this.ownerNode.getName()); + + if (this.getParametersService != null) { + this.getParametersService.dispose(); + } + if (this.getParameterTypesService != null) { + this.getParameterTypesService.dispose(); + } + if (this.setParametersService != null) { + this.setParametersService.dispose(); + } + if (this.setParametersAtomicallyService != null) { + this.setParametersAtomicallyService.dispose(); + } + if (this.describeParametersService != null) { + this.describeParametersService.dispose(); + } + if (this.listParametersService != null) { + this.listParametersService.dispose(); + } + if (this.eventparameterPublisher != null) { + this.eventparameterPublisher.dispose(); + } + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterVariant.java b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterVariant.java new file mode 100644 index 00000000..5aca7009 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/ParameterVariant.java @@ -0,0 +1,154 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.parameter; + +import java.util.List; + +import rcl_interfaces.msg.Parameter; +import rcl_interfaces.msg.ParameterType; +import rcl_interfaces.msg.ParameterValue; + +/** + * Parameter Variant. + * + * @param Parameter Type. + */ +public class ParameterVariant { + + private final String name; + private T value; + + public ParameterVariant(String name, T value) { + this.name = name; + this.value = value; + } + + /** + * @return the key + */ + public String getName() { + return name; + } + + /** + * @param value the value to set + */ + public void setValue(T value) { + this.value = value; + } + + /** + * @return the value + */ + public T getValue() { + return value; + } + + public String getTypeName() { + return this.value.getClass().getSimpleName(); + } + + public String valueToString() { + return this.value.toString(); + } + + @SuppressWarnings("unchecked") + public ParameterValue toParameterValue() { + ParameterValue p = new ParameterValue(); + + if (this.value == null) { + p.setType(ParameterType.PARAMETER_NOT_SET); + } else { + if (Boolean.class.equals(this.value.getClass())) { + p.setBoolValue((Boolean) this.value); + p.setType(ParameterType.PARAMETER_BOOL); + } else + + if (List.class.equals(this.value.getClass())) { + p.setBytesValue((List) this.value); + p.setType(ParameterType.PARAMETER_BYTES); + } else + + if (Double.class.equals(this.value.getClass())) { + p.setDoubleValue((Double) this.value); + p.setType(ParameterType.PARAMETER_DOUBLE); + } else + + if (Long.class.equals(this.value.getClass())) { + p.setIntegerValue((Long) this.value); + p.setType(ParameterType.PARAMETER_INTEGER); + } else + + if (String.class.equals(this.value.getClass())) { + p.setStringValue((String) this.value); + p.setType(ParameterType.PARAMETER_STRING); + } + } + + return p; + } + + public Parameter toParameter() { + Parameter result = new Parameter(); + result.setName(this.name); + result.setValue(this.toParameterValue()); + + return result; + } + + public static ParameterVariant fromParameter(Parameter parameter) { + + ParameterVariant reuslt = null; + + if (ParameterType.PARAMETER_BOOL == parameter.getValue().getType()) { + reuslt = new ParameterVariant( + parameter.getName(), + parameter.getValue().getBoolValue()); + } else + + if (ParameterType.PARAMETER_BYTES == parameter.getValue().getType()) { + reuslt = new ParameterVariant>( + parameter.getName(), + parameter.getValue().getBytesValue()); + } else + + if (ParameterType.PARAMETER_DOUBLE == parameter.getValue().getType()) { + reuslt = new ParameterVariant( + parameter.getName(), + parameter.getValue().getDoubleValue()); + } else + + if (ParameterType.PARAMETER_INTEGER == parameter.getValue().getType()) { + reuslt = new ParameterVariant( + parameter.getName(), + parameter.getValue().getIntegerValue()); + } else + + if (ParameterType.PARAMETER_STRING == parameter.getValue().getType()) { + reuslt = new ParameterVariant( + parameter.getName(), + parameter.getValue().getStringValue()); + } else + + { + reuslt = new ParameterVariant( + parameter.getName(), + null); + } + + return reuslt; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/parameter/SyncParametersClient.java b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/SyncParametersClient.java new file mode 100644 index 00000000..6140f290 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/SyncParametersClient.java @@ -0,0 +1,313 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.parameter; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.concurrent.ExecutionException; +import java.util.concurrent.Future; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.service.Client; +import org.ros2.rcljava.node.topic.Consumer; +import org.ros2.rcljava.node.topic.Subscription; +import org.ros2.rcljava.node.topic.Topics; + +import rcl_interfaces.srv.GetParameters; +import rcl_interfaces.srv.GetParameters_Request; +import rcl_interfaces.srv.GetParameters_Response; +import rcl_interfaces.srv.GetParameterTypes; +import rcl_interfaces.srv.GetParameterTypes_Request; +import rcl_interfaces.srv.GetParameterTypes_Response; +import rcl_interfaces.srv.SetParameters; +import rcl_interfaces.srv.SetParameters_Request; +import rcl_interfaces.srv.SetParameters_Response; +import rcl_interfaces.srv.ListParameters; +import rcl_interfaces.srv.ListParameters_Request; +import rcl_interfaces.srv.ListParameters_Response; +import rcl_interfaces.srv.DescribeParameters; +import rcl_interfaces.srv.DescribeParameters_Request; +import rcl_interfaces.srv.DescribeParameters_Response; +import rcl_interfaces.msg.ListParametersResult; +import rcl_interfaces.msg.Parameter; +import rcl_interfaces.msg.ParameterDescriptor; +import rcl_interfaces.msg.ParameterEvent; +import rcl_interfaces.msg.ParameterValue; +import rcl_interfaces.msg.SetParametersResult; + +/** + * Sync Parameter Client. + * + */ +public class SyncParametersClient { + + private static final Logger logger = LoggerFactory.getLogger(SyncParametersClient.class); + + private final Node ownerNode; + private final String remoteNodeName; + private Client getParametersClient; + private Client getParameterTypesClient; + private Client setParametersClient; + private Client listParametersClient; + private Client describeParametersClient; + + public SyncParametersClient(final Node node) { + this(node, null, QoSProfile.PARAMETER); + } + + public SyncParametersClient(final Node node, String remoteNodeName) { + this(node, remoteNodeName, QoSProfile.PARAMETER); + } + + public SyncParametersClient(final Node node, final QoSProfile profileParameter) { + this(node, null, profileParameter); + } + + public SyncParametersClient(final Node node, final String remoteNodeName, final QoSProfile profileParameter) { + this.ownerNode = node; + if (remoteNodeName == null || remoteNodeName.equals("")) { + this.remoteNodeName = node.getName(); + } else { + this.remoteNodeName = remoteNodeName; + } + + logger.debug(String.format("Create parameter client for %s", this.remoteNodeName)); + + try { + this.getParametersClient = this.ownerNode.createClient( + GetParameters.class, + String.format(ParameterService.TOPIC_GETPARAMETERS, this.remoteNodeName), + profileParameter); + this.getParameterTypesClient = this.ownerNode.createClient( + GetParameterTypes.class, + String.format(ParameterService.TOPIC_GETPARAMETERTYPES, this.remoteNodeName), + profileParameter); + this.setParametersClient = this.ownerNode.createClient( + SetParameters.class, + String.format(ParameterService.TOPIC_SETPARAMETERS, this.remoteNodeName), + profileParameter); + this.listParametersClient = this.ownerNode.createClient( + ListParameters.class, + String.format(ParameterService.TOPIC_LISTPARAMETERS, this.remoteNodeName), + profileParameter); + this.describeParametersClient = this.ownerNode.createClient( + DescribeParameters.class, + String.format(ParameterService.TOPIC_DESCRIBEPARAMETERS, this.remoteNodeName), + profileParameter); + } catch (Exception e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + /** + * Safely destroy the underlying ROS2 subscriber structure. + */ + public void dispose() { + logger.debug("Destroy parameter client : " + this.remoteNodeName); + + this.getParametersClient.dispose(); + this.getParameterTypesClient.dispose(); + this.setParametersClient.dispose(); + this.listParametersClient.dispose(); + this.describeParametersClient.dispose(); + } + + public List> getParameters(final List list) { + List> result = null; + + GetParameters_Request request = new GetParameters_Request(); + request.setNames(list); + + // Call service... + Future future = this.getParametersClient.sendRequest(request); + + if (future != null) { + try { + logger.debug("Call get Parameter service."); + List values = future.get().getValues(); + + result = new ArrayList>(); + for (int i = 0; i < values.size(); i++) { + ParameterValue parameterValue = values.get(i); + + Parameter parameter = new Parameter(); + parameter.setName(request.getNames().get(i)); + parameter.setValue(parameterValue); + + ParameterVariant parameterVariant = ParameterVariant.fromParameter(parameter); + result.add(parameterVariant); + } + + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } catch (ExecutionException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } else { + logger.error("getParameters was interrupted. Exiting."); + } + + return result; + } + + public List getParameterTypes(final List parameterNames) { + List result = null; + + GetParameterTypes_Request request = new GetParameterTypes_Request(); + request.setNames(parameterNames); + + // Call service... + Future future = this.getParameterTypesClient.sendRequest(request); + + if (future != null) { + try { + logger.debug("Call get Type of Parameter service."); + result = future.get().getTypes(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } catch (ExecutionException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } else { + logger.error("getParameterTypes was interrupted. Exiting."); + } + + return result; + } + + public boolean hasParameter(final String parameterName) { + List finded = Arrays.asList(parameterName); + logger.debug("Has Parameter service."); + ListParametersResult result = this.listParameters(finded, 1); + return result.getNames().size() > 0; + } + + public ListParametersResult listParameters(final List prefixes, int depth) { + ListParametersResult result = null; + + // Set request. + ListParameters_Request request = new ListParameters_Request(); + request.setPrefixes(prefixes); + request.setDepth(depth); + + // Call service... + Future future = this.listParametersClient.sendRequest(request); + + if (future != null) { + try { + logger.debug("Call list of Parameter service."); + result = future.get().getResult(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } catch (ExecutionException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } else { + logger.error("listParameters was interrupted. Exiting."); + } + + return result; + } + + public List setParameters(final List> list) { + List result = null; + + // Set request. + SetParameters_Request request = new SetParameters_Request(); + + List convList = new ArrayList(); + for (ParameterVariant parameterVariant : list) { + Parameter param = parameterVariant.toParameter(); + convList.add(param); + } + request.setParameters(convList); + + // Call service... + Future future = this.setParametersClient.sendRequest(request); + + if (future != null) { + try { + logger.debug("Call set Parameter service."); + result = future.get().getResults(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } catch (ExecutionException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } else { + logger.error("setParameters was interrupted. Exiting."); + } + + return result; + } + + public List describeParametersClient(final List parameterNames) { + List result = null; + + DescribeParameters_Request request = new DescribeParameters_Request(); + request.setNames(parameterNames); + + // Call service... + Future future = this.describeParametersClient.sendRequest(request); + + if (future != null) { + try { + logger.debug("Call describe Parameter service."); + result = future.get().getDescriptors(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } catch (ExecutionException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } else { + logger.error("setParameters was interrupted. Exiting."); + } + + return result; + } + + public Subscription onParameterEvent(final ParameterEventConsumer parameterConsumer) { + Subscription sub_event = this.ownerNode.createSubscription( + ParameterEvent.class, + Topics.PARAM_EVENT, + new Consumer() { + @Override + public void accept(ParameterEvent msg) { + parameterConsumer.onEvent(msg); + } + }, + QoSProfile.PARAMETER_EVENTS); + + return sub_event; + } + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/parameter/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/package-info.java new file mode 100644 index 00000000..334ed691 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/parameter/package-info.java @@ -0,0 +1,20 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 Parameter Stack. + */ +package org.ros2.rcljava.node.parameter; diff --git a/rcljava/src/main/java/org/ros2/rcljava/BiConsumer.java b/rcljava/src/main/java/org/ros2/rcljava/node/service/BiConsumer.java similarity index 91% rename from rcljava/src/main/java/org/ros2/rcljava/BiConsumer.java rename to rcljava/src/main/java/org/ros2/rcljava/node/service/BiConsumer.java index 2993abf4..1c5d3793 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/BiConsumer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/service/BiConsumer.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,8 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - -package org.ros2.rcljava; +package org.ros2.rcljava.node.service; /** * This is a copy of {@link java.util.funcion.BiConsumer} for platforms that @@ -23,6 +23,7 @@ * @param - the type of the second input to the operation */ public interface BiConsumer { + /** * Performs this operation on the given argument. * @@ -30,4 +31,5 @@ public interface BiConsumer { * @param input2 - the second input argument */ void accept(T input1, U input2); + } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/service/Client.java b/rcljava/src/main/java/org/ros2/rcljava/node/service/Client.java new file mode 100644 index 00000000..5fdd98fb --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/service/Client.java @@ -0,0 +1,174 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.service; + +import java.lang.ref.WeakReference; +import java.util.HashMap; +import java.util.Map; +import java.util.concurrent.Future; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.exception.NotImplementedException; +import org.ros2.rcljava.internal.IClient; +import org.ros2.rcljava.node.Node; + +/** + * Service Client. + * + * @param Service Type. + */ +public class Client implements IClient { + + // Loading JNI library. + static { + RCLJava.loadLibrary("rcljavanode_service_Client__" + RCLJava.getRMWIdentifier()); + } + + private static native void nativeSendClientRequest( + long clientHandle, + long sequenceNumber, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + Object requestMessage); + + + private final WeakReference nodeReference; + + /** Client Handler. */ + private final long clientHandle; + + private final Class serviceType; + private final String serviceName; + private long sequenceNumber = 0; + private Map> pendingRequests; + + private long requestFromJavaConverterHandle = 0; + private long requestToJavaConverterHandle = 0; + + private long responseFromJavaConverterHandle = 0; + private long responseToJavaConverterHandle = 0; + + private final Class requestType; + private final Class responseType; + + /** + * Constructor. + * + */ + public Client( + final WeakReference nodeReference, + final long clientHandle, + final Class serviceType, + final String serviceName, + final Class requestType, + final Class responseType, + final long requestFromJavaConverterHandle, + final long requestToJavaConverterHandle, + final long responseFromJavaConverterHandle, + final long responseToJavaConverterHandle) { + + if (nodeReference == null && clientHandle == 0) { + throw new RuntimeException("Need to provide active node with handle object"); + } + + this.nodeReference = nodeReference; + this.clientHandle = clientHandle; + + this.serviceType = serviceType; + this.serviceName = serviceName; + this.requestType = requestType; + this.responseType = responseType; + this.requestFromJavaConverterHandle = requestFromJavaConverterHandle; + this.requestToJavaConverterHandle = requestToJavaConverterHandle; + this.responseFromJavaConverterHandle = responseFromJavaConverterHandle; + this.responseToJavaConverterHandle = responseToJavaConverterHandle; + this.pendingRequests = new HashMap>(); + + this.nodeReference.get().getClients().add(this); + } + + @Override + public void dispose() { + this.nodeReference.get().getClients().remove(this); + } + + @Override + public final Future sendRequest(final U request) { + synchronized(this.pendingRequests) { + this.sequenceNumber++; + Client.nativeSendClientRequest( + this.clientHandle, + this.sequenceNumber, + this.requestFromJavaConverterHandle, + this.requestToJavaConverterHandle, + request); + RCLFuture future = new RCLFuture(this.nodeReference); + pendingRequests.put(sequenceNumber, future); + return future; + } + } + + @SuppressWarnings("unchecked") + public final void handleResponse(final RMWRequestId header,final V response) { + synchronized(pendingRequests) { + long sequenceNumber = header.sequenceNumber; + RCLFuture future = (RCLFuture) pendingRequests.remove(sequenceNumber); + future.set(response); + } + } + + public final Class getRequestType() { + return this.requestType; + } + + public final Class getResponseType() { + return this.responseType; + } + + public final String getServiceName() { + return this.serviceName; + } + + public final Class getServiceType() { + return this.serviceType; + } + + public final long getClientHandle() { + return this.clientHandle; + } + + public final long getRequestFromJavaConverterHandle() { + return this.requestFromJavaConverterHandle; + } + + public final long getRequestToJavaConverterHandle() { + return this.requestToJavaConverterHandle; + } + + public final long getResponseFromJavaConverterHandle() { + return this.responseFromJavaConverterHandle; + } + + public final long getResponseToJavaConverterHandle() { + return this.responseToJavaConverterHandle; + } + + public final boolean waitForService(final int i) { + //TODO + throw new NotImplementedException(); +// return false; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLFuture.java b/rcljava/src/main/java/org/ros2/rcljava/node/service/RCLFuture.java similarity index 91% rename from rcljava/src/main/java/org/ros2/rcljava/RCLFuture.java rename to rcljava/src/main/java/org/ros2/rcljava/node/service/RCLFuture.java index b1652724..d53e784d 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RCLFuture.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/service/RCLFuture.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,8 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - -package org.ros2.rcljava; +package org.ros2.rcljava.node.service; import java.lang.ref.WeakReference; import java.util.concurrent.ExecutionException; @@ -21,6 +21,9 @@ import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeoutException; +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.node.Node; + public class RCLFuture implements Future { private WeakReference nodeReference; @@ -31,6 +34,7 @@ public RCLFuture(final WeakReference nodeReference) { this.nodeReference = nodeReference; } + @Override public final V get() throws InterruptedException, ExecutionException { while (RCLJava.ok() && !isDone()) { Node node = nodeReference.get(); @@ -43,6 +47,7 @@ public final V get() throws InterruptedException, ExecutionException { return value; } + @Override public final V get(final long timeout, final TimeUnit unit) throws InterruptedException, ExecutionException, TimeoutException { if (isDone()) { @@ -77,14 +82,17 @@ public final V get(final long timeout, final TimeUnit unit) throws throw new InterruptedException(); } + @Override public final boolean isDone() { return done; } + @Override public final boolean isCancelled() { return false; } + @Override public final boolean cancel(final boolean mayInterruptIfRunning) { return false; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/RMWRequestId.java b/rcljava/src/main/java/org/ros2/rcljava/node/service/RMWRequestId.java similarity index 75% rename from rcljava/src/main/java/org/ros2/rcljava/RMWRequestId.java rename to rcljava/src/main/java/org/ros2/rcljava/node/service/RMWRequestId.java index eb3e9ff4..32b17b93 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/RMWRequestId.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/service/RMWRequestId.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,10 +13,14 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +package org.ros2.rcljava.node.service; -package org.ros2.rcljava; - +/** + * Service Request ID. + */ public class RMWRequestId { - public byte[] writerGUID = new byte[16]; - public long sequenceNumber; + + public byte[] writerGUID = new byte[16]; + public long sequenceNumber; + } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/service/Service.java b/rcljava/src/main/java/org/ros2/rcljava/node/service/Service.java new file mode 100644 index 00000000..eb89c627 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/service/Service.java @@ -0,0 +1,146 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.service; + +import org.ros2.rcljava.internal.IService; +import org.ros2.rcljava.node.Node; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +/** + * Service Server. + * + * @param Service Type. + */ +public class Service implements IService { + + private static final Logger logger = LoggerFactory.getLogger(Service.class); + + public static final String SCHEME = "rostopic://"; + + /** Name of the service */ + private final String serviceName; + + /** Node owner. */ + private final Node ownerNode; + + /** Service Handler. */ + private final long serviceHandle; + + private final Class serviceType; + + private final TriConsumer callback; + + private final long requestFromJavaConverterHandle; + private final long requestToJavaConverterHandle; + + private final long responseFromJavaConverterHandle; + private final long responseToJavaConverterHandle; + + private final Class requestType; + private final Class responseType; + + /** + * Constructor. + * + * @param nodeHandle + * @param serviceName + */ + public Service( + final Node node, + final long serviceHandle, + final Class serviceType, + final String serviceName, + final TriConsumer callback, + final Class requestType, + final Class responseType, + final long requestFromJavaConverterHandle, + final long requestToJavaConverterHandle, + final long responseFromJavaConverterHandle, + final long responseToJavaConverterHandle) { + + if (node == null && serviceHandle == 0) { + throw new RuntimeException("Need to provide active node with handle object"); + } + + Service.logger.debug("Init Service stack : " + serviceName); + + this.ownerNode = node; + this.serviceHandle = serviceHandle; + + this.serviceType = serviceType; + this.serviceName = serviceName; + this.callback = callback; + this.requestType = requestType; + this.responseType = responseType; + this.requestFromJavaConverterHandle = requestFromJavaConverterHandle; + this.requestToJavaConverterHandle = requestToJavaConverterHandle; + this.responseFromJavaConverterHandle = responseFromJavaConverterHandle; + this.responseToJavaConverterHandle = responseToJavaConverterHandle; + + this.ownerNode.getServices().add(this); + } + + @Override + public void dispose() { + Service.logger.debug("Destroy Service stack : " + this.serviceName); + this.ownerNode.getServices().remove(this); + } + + public void sendResponse() { + + } + + public String getServiceName() { + return this.serviceName; + } + + public final TriConsumer getCallback() { + return callback; + } + + public final Class getServiceType() { + return serviceType; + } + + public final long getServiceHandle() { + return this.serviceHandle; + } + + public final long getRequestFromJavaConverterHandle() { + return this.requestFromJavaConverterHandle; + } + + public final long getRequestToJavaConverterHandle() { + return this.requestToJavaConverterHandle; + } + + public final long getResponseFromJavaConverterHandle() { + return this.responseFromJavaConverterHandle; + } + + public final long getResponseToJavaConverterHandle() { + return this.responseToJavaConverterHandle; + } + + public final Class getRequestType() { + return this.requestType; + } + + public final Class getResponseType() { + return this.responseType; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/TriConsumer.java b/rcljava/src/main/java/org/ros2/rcljava/node/service/TriConsumer.java similarity index 91% rename from rcljava/src/main/java/org/ros2/rcljava/TriConsumer.java rename to rcljava/src/main/java/org/ros2/rcljava/node/service/TriConsumer.java index 886e45b2..1594c234 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/TriConsumer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/service/TriConsumer.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,8 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - -package org.ros2.rcljava; +package org.ros2.rcljava.node.service; /** * Based on {@link java.util.funcion.BiConsumer}. @@ -23,6 +23,7 @@ * @param - the type of the third input to the operation */ public interface TriConsumer { + /** * Performs this operation on the given argument. * @@ -31,4 +32,5 @@ public interface TriConsumer { * @param input3 - the third input argument */ void accept(T input1, U input2, V input3); + } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/service/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/node/service/package-info.java new file mode 100644 index 00000000..6b472301 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/service/package-info.java @@ -0,0 +1,20 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 Service Stack. + */ +package org.ros2.rcljava.node.service; diff --git a/rcljava/src/main/java/org/ros2/rcljava/Consumer.java b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Consumer.java similarity index 69% rename from rcljava/src/main/java/org/ros2/rcljava/Consumer.java rename to rcljava/src/main/java/org/ros2/rcljava/node/topic/Consumer.java index 89eca567..0c70896b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/Consumer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Consumer.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,8 +13,9 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +package org.ros2.rcljava.node.topic; -package org.ros2.rcljava; +import org.ros2.rcljava.internal.message.Message; /** * This is a copy of {@link java.util.funcion.Consumer} for platforms that don't @@ -21,11 +23,12 @@ * * @param - the type of the input to the operation */ -public interface Consumer { - /** - * Performs this operation on the given argument. - * - * @param input - the input argument - */ - void accept(T input); +public interface Consumer { + + /** + * Performs this operation on the given argument. + * + * @param message - the input argument + */ + void accept(T message); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/topic/Publisher.java b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Publisher.java new file mode 100644 index 00000000..79fe70f4 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Publisher.java @@ -0,0 +1,151 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.topic; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import org.ros2.rcljava.qos.QoSProfile; +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.internal.IPublisher; +import org.ros2.rcljava.node.Node; + +/** + * This class serves as a bridge between ROS2's rcl_publisher_t and RCLJava. + * A Publisher must be created via + * @{link Node#createPublisher(Class<T>, String)} + * + * @param The type of the messages that this publisher will publish. + */ +public class Publisher implements IPublisher { + + private static final Logger logger = LoggerFactory.getLogger(Publisher.class); + + static { + RCLJava.loadLibrary("rcljavanode_topic_Publisher__" + RCLJava.getRMWIdentifier()); + } + + /** + * Node owner. + */ + private final Node ownerNode; + + /** + * An integer that represents a pointer to the underlying ROS2 publisher + * structure (rcl_publisher_t). + */ + private final long publisherHandle; + + /** Message Type. */ + private final Class messageType; + + /** + * The topic to which this publisher will publish messages. + */ + private final String topic; + + /** Quality of Service profil. */ + private final QoSProfile qosProfile; + + // Native call. + /** + * Publish a message via the underlying ROS2 mechanisms. + * + * @param The type of the messages that this publisher will publish. + * @param publisherHandle A pointer to the underlying ROS2 publisher + * structure, as an integer. Must not be zero. + * @param message An instance of the <T> parameter. + */ + private static native void nativePublish(long publisherHandle, T message); + + /** + * Destroy a ROS2 publisher (rcl_publisher_t). + * + * @param nodeHandle A pointer to the underlying ROS2 node structure that + * created this subscription, as an integer. Must not be zero. + * @param publisherHandle A pointer to the underlying ROS2 publisher + * structure, as an integer. Must not be zero. + */ + private static native void nativeDispose(long nodeHandle, long publisherHandle); + + /** + * Constructor. + * + * @param nodeHandle A pointer to the underlying ROS2 node structure that + * created this subscription, as an integer. Must not be zero. + * @param publisherHandle A pointer to the underlying ROS2 publisher + * structure, as an integer. Must not be zero. + * @param topic The topic to which this publisher will publish messages. + * @param qos Quality of Service profile. + */ + public Publisher(final Node node, final long publisherHandle, final Class messageType, final String topic, final QoSProfile qosProfile) { + if (node == null && publisherHandle == 0) { + throw new RuntimeException("Need to provide active node with handle object"); + } + + this.ownerNode = node; + this.publisherHandle = publisherHandle; + this.messageType = messageType; + this.topic = topic; + this.qosProfile = qosProfile; + + this.ownerNode.getPublishers().add(this); + } + + /** + * Publish a message. + * + * @param message An instance of the <T> parameter. + */ + @Override + public void publish(final T message) { + Publisher.nativePublish(this.publisherHandle, message); + } + + /** + * Get message type. + * @return + */ + public final Class getMsgType() { + return this.messageType; + } + + /** + * Get topic name. + * @return + */ + public final String getTopic() { + return this.topic; + } + + public final Node getNode() { + return this.ownerNode; + } + + public final long getPublisherHandle() { + return this.publisherHandle; + } + + /** + * Safely destroy the underlying ROS2 publisher structure. + */ + @Override + public void dispose() { + Publisher.logger.debug("Destroy Publisher of topic : " + this.topic); + this.ownerNode.getPublishers().remove(this); + Publisher.nativeDispose(this.ownerNode.getNodeHandle(), this.publisherHandle); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/topic/Subscription.java b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Subscription.java new file mode 100644 index 00000000..e916ab5f --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Subscription.java @@ -0,0 +1,151 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.topic; + +import org.ros2.rcljava.qos.QoSProfile; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.ros2.rcljava.internal.ISubscription; +import org.ros2.rcljava.node.Node; + +/** + * This class serves as a bridge between ROS2's rcl_subscription_t and RCLJava. + * A Subscription must be created via + * @{link Node#createSubscription(Class<T>, String, Consumer<T>)} + * + * @param The type of the messages that this subscription will receive. + */ +public class Subscription implements ISubscription { + + private static final Logger logger = LoggerFactory.getLogger(Subscription.class); + + /** + * Node owner. + */ + private final Node ownerNode; + + /** + * An integer that represents a pointer to the underlying ROS2 subscription structure (rcl_subsription_t). + */ + private final long subscriptionHandle; + + /** + * The class of the messages that this subscription may receive. + */ + private final Class messageType; + + /** + * The topic to which this subscription is subscribed. + */ + private final String topic; + + /** + * The callback function that will be triggered when a new message is + * received. + */ + private final Consumer callback; + + /** Quality of Service profile. */ + private final QoSProfile qosProfile; + + // Native call. + //private static native void nativeDispose(long nodeHandle, long publisherHandle); + + /** + * Constructor. + * + * @param nodeHandle A pointer to the underlying ROS2 node structure that + * created this subscription, as an integer. Must not be zero. + * @param subscriptionHandle A pointer to the underlying ROS2 subscription + * structure, as an integer. Must not be zero. + * @param messageType The Class of the messages that this + * subscription will receive. We need this because of Java's type erasure, + * which doesn't allow us to use the generic parameter of + * @{link org.ros2.rcljava.Subscription} directly. + * @param topic The topic to which this subscription will be subscribed. + * @param callback The callback function that will be triggered when a new + * message is received. + */ + public Subscription( + final Node node, + final long subscriptionHandle, + final Class messageType, + final String topic, + final Consumer callback, + final QoSProfile qosProfile) { + + if (node == null && subscriptionHandle == 0) { + throw new RuntimeException("Need to provide active node with handle object"); + } + + this.ownerNode = node; + this.subscriptionHandle = subscriptionHandle; + this.messageType = messageType; + this.topic = topic; + this.callback = callback; + this.qosProfile = qosProfile; + + this.ownerNode.getSubscriptions().add(this); + } + + public final Node getNode() { + return this.ownerNode; + } + + /** + * @return The callback function that this subscription will trigger when a message is received. + */ + public final Consumer getCallback() { + return this.callback; + } + + /** + * @return The pointer to the underlying ROS2 subscription structure. + */ + public final long getSubscriptionHandle() { + return this.subscriptionHandle; + } + + /** + * @return The type of the messages that this subscription may receive. + */ + public final Class getMessageType() { + return this.messageType; + } + + /** + * Get topic name. + * @return + */ + public final String getTopic() { + return this.topic; + } + + /** + * Get QOS Profile + * @return + */ + public final QoSProfile getQosProfile() { + return qosProfile; + } + + @Override + public void dispose() { + Subscription.logger.debug("Destroy Subscription of topic : " + this.topic); + this.ownerNode.getSubscriptions().remove(this); + // Subscription.nativeDispose(this.nodeHandle, this.subscriptionHandle); + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/topic/Topics.java b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Topics.java new file mode 100644 index 00000000..73916ce7 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/topic/Topics.java @@ -0,0 +1,31 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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.node.topic; + +/** + * Topics built-in. + */ +public class Topics { + + /** Standard ros2 out. */ + public static final String ROSOUT = "/rosout"; + + /** Parameter events topic. */ + public static final String PARAM_EVENT = "~/parameter_events"; + + public static final String SCHEME = "rostopic://"; + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/topic/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/node/topic/package-info.java new file mode 100644 index 00000000..df557043 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/node/topic/package-info.java @@ -0,0 +1,20 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 Topic Stack. + */ +package org.ros2.rcljava.node.topic; diff --git a/rcljava/src/main/java/org/ros2/rcljava/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/package-info.java index 8733530e..e91ca6f8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/package-info.java +++ b/rcljava/src/main/java/org/ros2/rcljava/package-info.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,6 +15,6 @@ */ /** - * An API for using ROS2 with Java. + * An API for using ROS2 with Java (aka rcljava). */ package org.ros2.rcljava; 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 3b9f5ee6..3ca60d66 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/QoSProfile.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,89 +13,111 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava.qos; import org.ros2.rcljava.qos.policies.Durability; import org.ros2.rcljava.qos.policies.History; -import org.ros2.rcljava.qos.policies.QoSPolicy; import org.ros2.rcljava.qos.policies.Reliability; +/** + * Quality of Service profile. + */ public class QoSProfile { - private final History history; - - private final int depth; - - private final Reliability reliability; - - private final Durability durability; - - public QoSProfile(History history, int depth, Reliability reliability, - Durability durability) { - this.history = history; - this.depth = depth; - this.reliability = reliability; - this.durability = durability; - } - - public final History getHistory() { - return this.history; - } - - public final int getDepth() { - return this.depth; - } - - public final Reliability getReliability() { - return this.reliability; - } - - public final Durability getDurability() { - return this.durability; - } - - public static final int DEPTH_SYSTEM_DEFAULT = 0; - - public static final QoSProfile SENSOR_DATA = new QoSProfile( - History.KEEP_LAST, - 5, - Reliability.BEST_EFFORT, - Durability.SYSTEM_DEFAULT - ); - - public static final QoSProfile PARAMETER = new QoSProfile( - History.KEEP_LAST, - 1000, - Reliability.RELIABLE, - Durability.SYSTEM_DEFAULT - ); - - public static final QoSProfile DEFAULT = new QoSProfile( - History.KEEP_ALL, - 10, - Reliability.RELIABLE, - Durability.SYSTEM_DEFAULT - ); - - public static final QoSProfile SERVICES_DEFAULT = new QoSProfile( - History.KEEP_LAST, - 10, - Reliability.RELIABLE, - Durability.TRANSIENT_LOCAL - ); - - public static final QoSProfile PARAMETER_EVENTS = new QoSProfile( - History.KEEP_ALL, - 1000, - Reliability.RELIABLE, - Durability.SYSTEM_DEFAULT - ); - - public static final QoSProfile SYSTEM_DEFAULT = new QoSProfile( - History.SYSTEM_DEFAULT, - DEPTH_SYSTEM_DEFAULT, - Reliability.SYSTEM_DEFAULT, - Durability.SYSTEM_DEFAULT - ); + /** + * History mode. + */ + private final History history; + + /** + * Depth. + */ + private final int depth; + + /** + * Reliability. + */ + private final Reliability reliability; + + /** + * Durability. + */ + private final Durability durability; + + /** + * Constructor. + * @param history + * @param depth + * @param reliability + * @param durability + */ + public QoSProfile(History history, int depth, Reliability reliability, Durability durability) { + this.history = history; + this.depth = depth; + this.reliability = reliability; + this.durability = durability; + } + + public final History getHistory() { + return this.history; + } + + public final int getDepth() { + return this.depth; + } + + public final Reliability getReliability() { + return this.reliability; + } + + public final Durability getDurability() { + return this.durability; + } + + public static final int DEPTH_SYSTEM_DEFAULT = 0; + + public static final QoSProfile SENSOR_DATA = new QoSProfile( + History.KEEP_LAST, + 5, + Reliability.BEST_EFFORT, + Durability.SYSTEM_DEFAULT); + + /** + * Default for Parameter layer. + */ + public static final QoSProfile PARAMETER = new QoSProfile( + History.KEEP_LAST, + 1000, + Reliability.RELIABLE, + Durability.SYSTEM_DEFAULT); + + /** + * Default for Sub/Pub layer. + */ + public static final QoSProfile DEFAULT = new QoSProfile( + History.KEEP_ALL, + 10, + Reliability.RELIABLE, + Durability.SYSTEM_DEFAULT); + + /** + * Default for Service layer. + */ + public static final QoSProfile SERVICES_DEFAULT = new QoSProfile( + History.KEEP_LAST, + 10, + Reliability.RELIABLE, + Durability.TRANSIENT_LOCAL); + + public static final QoSProfile PARAMETER_EVENTS = new QoSProfile( + History.KEEP_ALL, + 1000, + Reliability.RELIABLE, + Durability.SYSTEM_DEFAULT); + + public static final QoSProfile SYSTEM_DEFAULT = new QoSProfile( + History.SYSTEM_DEFAULT, + DEPTH_SYSTEM_DEFAULT, + Reliability.SYSTEM_DEFAULT, + Durability.SYSTEM_DEFAULT); } 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 d7796c42..d826ad3b 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 @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,9 +13,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava.qos.policies; +/** + * Durability enum. + * + */ public enum Durability implements QoSPolicy { SYSTEM_DEFAULT(0), TRANSIENT_LOCAL(1), 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 c8903140..3443086b 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 @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,9 +13,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava.qos.policies; +/** + * History enum. + * + */ public enum History implements QoSPolicy { SYSTEM_DEFAULT(0), KEEP_LAST(1), diff --git a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/QoSPolicy.java b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/QoSPolicy.java index d10d597d..1b5b3030 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/qos/policies/QoSPolicy.java +++ b/rcljava/src/main/java/org/ros2/rcljava/qos/policies/QoSPolicy.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,7 +13,6 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava.qos.policies; public interface QoSPolicy { 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 b15d6cde..8b066c06 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 @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,9 +13,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava.qos.policies; +/** + * Reliability enum + * + */ public enum Reliability implements QoSPolicy { SYSTEM_DEFAULT(0), RELIABLE(1), diff --git a/rcljava/src/main/java/org/ros2/rcljava/time/package-info.java b/rcljava/src/main/java/org/ros2/rcljava/time/package-info.java new file mode 100644 index 00000000..3f2bbe69 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/time/package-info.java @@ -0,0 +1,20 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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. + */ + +/** + * ROS2 Time Stack. + */ +package org.ros2.rcljava.time; diff --git a/rcljava/src/test/java/org/ros2/rcljava/GraphNameTest.java b/rcljava/src/test/java/org/ros2/rcljava/GraphNameTest.java new file mode 100644 index 00000000..1cab307d --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/GraphNameTest.java @@ -0,0 +1,181 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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; + +import org.apache.log4j.BasicConfigurator; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Test; +import org.ros2.rcljava.namespace.GraphName; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.topic.Topics; + +public class GraphNameTest { + + @BeforeClass + public static void beforeClass() { + BasicConfigurator.resetConfiguration(); + BasicConfigurator.configure(); + } + + @Test + public final void testIsTopicName() { + // @see http://design.ros2.org/articles/topic_and_service_names.html + + // must not be empty + Assert.assertFalse("Topic must not be null.", GraphName.isValidTopic(null)); + Assert.assertFalse("Topic must not be empty.", GraphName.isValidTopic("")); + + // may contain alphanumeric characters ([0-9|a-z|A-Z]), underscores (_), or forward slashes (/) +// Assert.assertFalse("Topic can be contains valid charactere.", GraphName.isValidTopic("&é'(-èçà)=#[|\^@]^$ù*,;:!£%µ?.§")); + Assert.assertTrue("Topic may contains valid charactere.", GraphName.isValidTopic("~/_foo")); + Assert.assertTrue("Topic may contains valid charactere.", GraphName.isValidTopic("azertyuiopqsdfghjklmwxcvbn1234567890")); + + // may use balanced curly braces ({}) for substitutions + Assert.assertTrue("Topic may use balanced curly braces.", GraphName.isValidTopic("{bar}")); + + // may start with a tilde (~), the private namespace substitution character + Assert.assertTrue("Topic may start with a tilde (~)", GraphName.isValidTopic("~/foo")); + Assert.assertFalse("Topic may start with a tilde (~).", GraphName.isValidTopic("foo~")); + + // must not start with a numeric character ([0-9]) + Assert.assertFalse("Topic must not start with a numeric character ([0-9]).", GraphName.isValidTopic("8foo")); + + // must not contain any number of repeated underscores (_) + Assert.assertFalse("Topic must not contain any number of repeated underscores (_).", GraphName.isValidTopic("foo__bar")); + + // must not end with an underscore (_) + Assert.assertFalse("Topic must not end with an underscore (_).", GraphName.isValidTopic("foo_")); + + // must not have an underscore (_) followed by a forward slash (/), i.e. _/ + Assert.assertFalse("Topic must not have an underscore (_) followed by a forward slash.", GraphName.isValidTopic("foo_/bar")); + + // must not end with a forward slash (/) + Assert.assertFalse("Topic must not end with a forward slash (/).", GraphName.isValidTopic("foo/bar/")); + + // must not contain any number of repeated forward slashes (/) + Assert.assertFalse("Topic must not contain any number of repeated forward slashes (/).", GraphName.isValidTopic("foo//bar")); + + // must separate a tilde (~) from the rest of the name with a forward slash (/), i.e. ~/foo not ~foo + Assert.assertFalse("Topic must separate a tilde (~) from the rest of the name with a forward slash (/).", GraphName.isValidTopic("~foo")); + + // must have balanced curly braces ({}) when used, i.e. {sub}/foo but not {sub/foo nor /foo} + Assert.assertTrue("Topic must have balanced curly braces ({}) when used.", GraphName.isValidTopic("~/foo")); + + // Test case Valid + Assert.assertTrue(GraphName.isValidTopic("foo")); + Assert.assertTrue(GraphName.isValidTopic("abc123")); + Assert.assertTrue(GraphName.isValidTopic("_foo")); + Assert.assertTrue(GraphName.isValidTopic("Foo")); + Assert.assertTrue(GraphName.isValidTopic("BAR")); + Assert.assertTrue(GraphName.isValidTopic("~")); + Assert.assertTrue(GraphName.isValidTopic("foo/bar")); + Assert.assertTrue(GraphName.isValidTopic("~/foo")); + Assert.assertTrue(GraphName.isValidTopic("{foo}_bar")); + Assert.assertTrue(GraphName.isValidTopic("foo/{ping}/bar")); + Assert.assertTrue(GraphName.isValidTopic(Topics.SCHEME + "/foo")); + Assert.assertTrue(GraphName.isValidTopic(Topics.SCHEME + "foo/bar")); + + // Test case not valid + Assert.assertFalse(GraphName.isValidTopic("123abc")); + Assert.assertFalse(GraphName.isValidTopic("123")); + Assert.assertFalse(GraphName.isValidTopic("__foo")); + Assert.assertFalse(GraphName.isValidTopic("foo__bar")); + Assert.assertFalse(GraphName.isValidTopic("foo bar")); + Assert.assertFalse(GraphName.isValidTopic("foo__")); + Assert.assertFalse(GraphName.isValidTopic(" ")); + Assert.assertFalse(GraphName.isValidTopic("foo_")); + Assert.assertFalse(GraphName.isValidTopic("foo//bar")); + Assert.assertFalse(GraphName.isValidTopic("foo/")); + Assert.assertFalse(GraphName.isValidTopic("foo_/bar ")); + Assert.assertFalse(GraphName.isValidTopic("~foo")); + Assert.assertFalse(GraphName.isValidTopic("foo~")); + Assert.assertFalse(GraphName.isValidTopic("foo~/bar")); + Assert.assertFalse(GraphName.isValidTopic("foo/~bar")); + Assert.assertFalse(GraphName.isValidTopic("/_/bar")); + Assert.assertFalse(GraphName.isValidTopic("_")); + Assert.assertFalse(GraphName.isValidTopic("_/_bar")); + Assert.assertFalse(GraphName.isValidTopic("/~")); + Assert.assertFalse(GraphName.isValidTopic("foo/~/bar")); + } + + @Test + public final void testIsSubstitution() { + // must not be empty + Assert.assertFalse("Substitution must not be null.", GraphName.isValidSubstitution(null)); + Assert.assertFalse("Substitution must not be empty.", GraphName.isValidSubstitution("")); + + // may contain alphanumeric characters ([0-9|a-z|A-Z]) and underscores (_) + Assert.assertTrue("Substitution may contain alphanumeric characters.", GraphName.isValidSubstitution("foo_bar")); + Assert.assertTrue("Substitution may contain alphanumeric characters.", GraphName.isValidSubstitution("azertyuiopqsdfghjklmwxcvbn1234567890")); + + // must not start with a numeric character ([0-9]) + Assert.assertFalse("Topic must not start with a numeric character ([0-9]).", GraphName.isValidTopic("8foo")); + + // are unlike topic and service names in that they: + // may start with or end with underscores (_) //TODO (theos) For me risky (case far_{foo} with foo=_baz ) !! + Assert.assertTrue("Substitution may start with underscores.", GraphName.isValidSubstitution("_foo")); +// Assert.assertTrue("Substitution may end with underscores.", GraphName.isValidSubstitution("foo_")); + + // may contain repeated underscores, e.g. __ //TODO (theos) For me not valid !! + Assert.assertTrue("Substitution may contain repeated underscores.", GraphName.isValidSubstitution("foo__bar")); + } + + @Test + public final void testIsTopicFQN() { + // must start with a forward slash (/), i.e. they must be absolute + Assert.assertTrue("FQN must start with a forward slash.", GraphName.isValideFQDN("/foo")); + Assert.assertFalse("FQN must start with a forward slash.", GraphName.isValideFQDN("foo")); + + // must not contain tilde (~) or curly braces ({}) + Assert.assertFalse("FQN must not contain tilde.", GraphName.isValideFQDN("~foo")); + Assert.assertFalse("FQN must not contain curly braces.", GraphName.isValideFQDN("foo/{bar}")); + + Assert.assertTrue(GraphName.isValideFQDN("/foo")); + Assert.assertTrue(GraphName.isValideFQDN("/bar/baz")); + Assert.assertTrue(GraphName.isValideFQDN(Topics.SCHEME + "/ping")); + Assert.assertTrue(GraphName.isValideFQDN("/_private/thing")); + Assert.assertTrue(GraphName.isValideFQDN("/public_namespace/_private/thing")); + } + + @Test + public final void testGetFullName() { + RCLJava.rclJavaInit(); + + Node node = RCLJava.createNode("my_node"); + Assert.assertEquals("/ping", GraphName.getFullName(node, "ping", null)); + Assert.assertEquals("/ping", GraphName.getFullName(node, "/ping", null)); + Assert.assertEquals("/my_node", GraphName.getFullName(node, "~", null)); + Assert.assertEquals("/my_node/ping", GraphName.getFullName(node, "~/ping", null)); + node.dispose(); + + node = RCLJava.createNode("my_ns", "my_node"); + Assert.assertEquals("/my_ns/ping", GraphName.getFullName(node, "ping", null)); + Assert.assertEquals("/ping", GraphName.getFullName(node, "/ping", null)); + Assert.assertEquals("/my_ns/my_node", GraphName.getFullName(node, "~", null)); + Assert.assertEquals("/my_ns/my_node/ping", GraphName.getFullName(node, "~/ping", null)); + node.dispose(); + + node = RCLJava.createNode("/my_ns/my_subns", "my_node"); + Assert.assertEquals("/my_ns/my_subns/ping", GraphName.getFullName(node, "ping", null)); + Assert.assertEquals("/ping", GraphName.getFullName(node, "/ping", null)); + Assert.assertEquals("/my_ns/my_subns/my_node", GraphName.getFullName(node, "~", null)); + Assert.assertEquals("/my_ns/my_subns/my_node/ping", GraphName.getFullName(node, "~/ping", null)); + node.dispose(); + + RCLJava.shutdown(); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/MessageTest.java b/rcljava/src/test/java/org/ros2/rcljava/MessageTest.java new file mode 100644 index 00000000..9dbd3593 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/MessageTest.java @@ -0,0 +1,758 @@ +/* Copyright 2016 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; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotEquals; +import static org.junit.Assert.assertTrue; + +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; + +import java.lang.ref.WeakReference; + +import java.util.Arrays; +import java.util.List; + +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.service.RCLFuture; +import org.ros2.rcljava.node.topic.Consumer; +import org.ros2.rcljava.node.topic.Publisher; +import org.ros2.rcljava.node.topic.Subscription; +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.internal.message.Message; + + +public class MessageTest { + + private Node node; + private rcljava.msg.Primitives primitives1; + private rcljava.msg.Primitives primitives2; + + private boolean boolValue1, boolValue2; + private byte byteValue1, byteValue2; + private char charValue1, charValue2; + private float float32Value1, float32Value2; + private double float64Value1, float64Value2; + private byte int8Value1, int8Value2; + private byte uint8Value1, uint8Value2; + private short int16Value1, int16Value2; + private short uint16Value1, uint16Value2; + private int int32Value1, int32Value2; + private int uint32Value1, uint32Value2; + private long int64Value1, int64Value2; + private long uint64Value1, uint64Value2; + private String stringValue1, stringValue2; + + boolean checkPrimitives(rcljava.msg.Primitives primitives, + boolean booleanValue, byte byteValue, char charValue, float float32Value, + double float64Value, byte int8Value, byte uint8Value, short int16Value, + short uint16Value, int int32Value, int uint32Value, long int64Value, + long uint64Value, String stringValue) { + + boolean result = true; + result = result && (primitives.getBoolValue() == booleanValue); + result = result && (primitives.getByteValue() == byteValue); + result = result && (primitives.getCharValue() == charValue); + result = result && (primitives.getFloat32Value() == float32Value); + result = result && (primitives.getFloat64Value() == float64Value); + result = result && (primitives.getInt8Value() == int8Value); + result = result && (primitives.getUint8Value() == uint8Value); + result = result && (primitives.getInt16Value() == int16Value); + result = result && (primitives.getUint16Value() == uint16Value); + result = result && (primitives.getInt32Value() == int32Value); + result = result && (primitives.getUint32Value() == uint32Value); + result = result && (primitives.getInt64Value() == int64Value); + result = result && (primitives.getUint64Value() == uint64Value); + result = result && (primitives.getStringValue().equals(stringValue)); + + return result; + } + + @BeforeClass + public static void setupOnce() throws Exception { + RCLJava.rclJavaInit(); + org.apache.log4j.BasicConfigurator.configure(); + } + + public class TestConsumer implements Consumer { + private final RCLFuture future; + + TestConsumer(final RCLFuture future) { + this.future = future; + } + + public final void accept(final T msg) { + if(!this.future.isDone()) { + this.future.set(msg); + } + } + } + + @Before + public void setUp() { + node = RCLJava.createNode("test_node"); + + primitives1 = new rcljava.msg.Primitives(); + primitives2 = new rcljava.msg.Primitives(); + + boolValue1 = true; + byteValue1 = (byte)123; + charValue1 = '\u0012'; + float32Value1 = 12.34f; + float64Value1 = 43.21; + int8Value1 = (byte)-12; + uint8Value1 = (byte)34; + int16Value1 = (short)-1234; + uint16Value1 = (short)4321; + int32Value1 = -75536; + uint32Value1 = 85536; + int64Value1 = -5294967296l; + uint64Value1 = 6294967296l; + stringValue1 = "hello world"; + + primitives1.setBoolValue(boolValue1); + primitives1.setByteValue(byteValue1); + primitives1.setCharValue(charValue1); + primitives1.setFloat32Value(float32Value1); + primitives1.setFloat64Value(float64Value1); + primitives1.setInt8Value(int8Value1); + primitives1.setUint8Value(uint8Value1); + primitives1.setInt16Value(int16Value1); + primitives1.setUint16Value(uint16Value1); + primitives1.setInt32Value(int32Value1); + primitives1.setUint32Value(uint32Value1); + primitives1.setInt64Value(int64Value1); + primitives1.setUint64Value(uint64Value1); + primitives1.setStringValue(stringValue1); + + boolValue2 = false; + byteValue2 = (byte)42; + charValue2 = '\u0021'; + float32Value2 = 13.34f; + float64Value2 = 44.21; + int8Value2 = (byte)-13; + uint8Value2 = (byte)35; + int16Value2 = (short)-1235; + uint16Value2 = (short)4321; + int32Value2 = -75536; + uint32Value2 = 85536; + int64Value2 = -5294967296l; + uint64Value2 = 6294967296l; + stringValue2 = "bye world"; + + primitives2.setBoolValue(boolValue2); + primitives2.setByteValue(byteValue2); + primitives2.setCharValue(charValue2); + primitives2.setFloat32Value(float32Value2); + primitives2.setFloat64Value(float64Value2); + primitives2.setInt8Value(int8Value2); + primitives2.setUint8Value(uint8Value2); + primitives2.setInt16Value(int16Value2); + primitives2.setUint16Value(uint16Value2); + primitives2.setInt32Value(int32Value2); + primitives2.setUint32Value(uint32Value2); + primitives2.setInt64Value(int64Value2); + primitives2.setUint64Value(uint64Value2); + primitives2.setStringValue(stringValue2); + } + + @Test + public final void testCreate() { + assertNotEquals(0, node.getNodeHandle()); + } + + @Test + public final void testPubSubStdString() throws Exception { + Publisher publisher = node + .createPublisher(std_msgs.msg.String.class, + "test_topic"); + + RCLFuture future = new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription(std_msgs.msg.String.class, + "test_topic", new TestConsumer(future)); + + std_msgs.msg.String msg = new std_msgs.msg.String(); + msg.setData("Hello"); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + std_msgs.msg.String value = future.get(); + assertEquals("Hello", value.getData()); + } + + @Test + public final void testPubSubBoundedArrayNested() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.BoundedArrayNested.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.BoundedArrayNested.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.BoundedArrayNested msg = new rcljava.msg.BoundedArrayNested(); + msg.setPrimitiveValues(Arrays.asList(new rcljava.msg.Primitives[] {primitives1, primitives2})); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + 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); + + assertTrue( + checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, + float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, + uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, + stringValue1)); + + assertTrue( + checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, + float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, + uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, + stringValue2)); + } + + @Test + public final void testPubSubBoundedArrayPrimitives() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.BoundedArrayPrimitives.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.BoundedArrayPrimitives.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.BoundedArrayPrimitives msg = new rcljava.msg.BoundedArrayPrimitives(); + + List boolValues = Arrays.asList(new Boolean[] {true, false, true}); + List byteValues = Arrays.asList(new Byte[] {123, 42}); + List charValues = Arrays.asList(new Character[] {'\u0012', '\u0021'}); + List float32Values = Arrays.asList(new Float[] {12.34f, 13.34f}); + List float64Values = Arrays.asList(new Double[] {43.21, 44.21}); + List int8Values = Arrays.asList(new Byte[] {-12, -13}); + List uint8Values = Arrays.asList(new Byte[] {34, 35}); + List int16Values = Arrays.asList(new Short[] {-1234, -1235}); + List uint16Values = Arrays.asList(new Short[] {4321, 4322}); + List int32Values = Arrays.asList(new Integer[] {-75536, -75537}); + List uint32Values = Arrays.asList(new Integer[] {85536, 85537}); + List int64Values = Arrays.asList(new Long[] {-5294967296l, -5294967297l}); + List uint64Values = Arrays.asList(new Long[] {6294967296l, 6294967297l}); + List stringValues = Arrays.asList(new String[] {"hello world", "bye world"}); + + msg.setBoolValues(boolValues); + msg.setByteValues(byteValues); + msg.setCharValues(charValues); + msg.setFloat32Values(float32Values); + msg.setFloat64Values(float64Values); + msg.setInt8Values(int8Values); + msg.setUint8Values(uint8Values); + msg.setInt16Values(int16Values); + msg.setUint16Values(uint16Values); + msg.setInt32Values(int32Values); + msg.setUint32Values(uint32Values); + msg.setInt64Values(int64Values); + msg.setUint64Values(uint64Values); + msg.setStringValues(stringValues); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + 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()); + } + + @Test + public final void testPubSubBuiltins() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.Builtins.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.Builtins.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.Builtins msg = new rcljava.msg.Builtins(); + + builtin_interfaces.msg.Duration duration = new builtin_interfaces.msg.Duration(); + duration.setSec(1234); + duration.setNanosec(4567); + + builtin_interfaces.msg.Time time = new builtin_interfaces.msg.Time(); + time.setSec(4321); + time.setNanosec(7654); + + msg.setDurationValue(duration); + msg.setTimeValue(time); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + rcljava.msg.Builtins value = future.get(); + builtin_interfaces.msg.Duration durationValue = value.getDurationValue(); + builtin_interfaces.msg.Time timeValue = value.getTimeValue(); + + assertEquals(1234, durationValue.getSec()); + assertEquals(4567, durationValue.getNanosec()); + + assertEquals(4321, timeValue.getSec()); + assertEquals(7654, timeValue.getNanosec()); + } + + @Test + public final void testPubSubDynamicArrayNested() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.DynamicArrayNested.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.DynamicArrayNested.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.DynamicArrayNested msg = new rcljava.msg.DynamicArrayNested(); + msg.setPrimitiveValues(Arrays.asList(new rcljava.msg.Primitives[] {primitives1, primitives2})); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + 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); + + assertTrue( + checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, + float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, + uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, + stringValue1)); + + assertTrue( + checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, + float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, + uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, + stringValue2)); + } + + @Test + public final void testPubSubDynamicArrayPrimitives() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.DynamicArrayPrimitives.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.DynamicArrayPrimitives.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.DynamicArrayPrimitives msg = new rcljava.msg.DynamicArrayPrimitives(); + + List boolValues = Arrays.asList(new Boolean[] {true, false, true}); + List byteValues = Arrays.asList(new Byte[] {123, 42}); + List charValues = Arrays.asList(new Character[] {'\u0012', '\u0021'}); + List float32Values = Arrays.asList(new Float[] {12.34f, 13.34f}); + List float64Values = Arrays.asList(new Double[] {43.21, 44.21}); + List int8Values = Arrays.asList(new Byte[] {-12, -13}); + List uint8Values = Arrays.asList(new Byte[] {34, 35}); + List int16Values = Arrays.asList(new Short[] {-1234, -1235}); + List uint16Values = Arrays.asList(new Short[] {4321, 4322}); + List int32Values = Arrays.asList(new Integer[] {-75536, -75537}); + List uint32Values = Arrays.asList(new Integer[] {85536, 85537}); + List int64Values = Arrays.asList(new Long[] {-5294967296l, -5294967297l}); + List uint64Values = Arrays.asList(new Long[] {6294967296l, 6294967297l}); + List stringValues = Arrays.asList(new String[] {"hello world", "bye world"}); + + msg.setBoolValues(boolValues); + msg.setByteValues(byteValues); + msg.setCharValues(charValues); + msg.setFloat32Values(float32Values); + msg.setFloat64Values(float64Values); + msg.setInt8Values(int8Values); + msg.setUint8Values(uint8Values); + msg.setInt16Values(int16Values); + msg.setUint16Values(uint16Values); + msg.setInt32Values(int32Values); + msg.setUint32Values(uint32Values); + msg.setInt64Values(int64Values); + msg.setUint64Values(uint64Values); + msg.setStringValues(stringValues); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + 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()); + } + + @Test + public final void testPubSubEmpty() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.Empty.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.Empty.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.Empty msg = new rcljava.msg.Empty(); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + rcljava.msg.Empty value = future.get(); + assertNotEquals(null, value); + } + + @Test + public final void testPubSubFieldsWithSameType() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.FieldsWithSameType.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.FieldsWithSameType.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.FieldsWithSameType msg = new rcljava.msg.FieldsWithSameType(); + + msg.setPrimitiveValues1(primitives1); + msg.setPrimitiveValues2(primitives2); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + rcljava.msg.FieldsWithSameType value = future.get(); + assertNotEquals(null, value.getPrimitiveValues1()); + assertNotEquals(null, value.getPrimitiveValues2()); + + rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues1(); + rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues2(); + + assertTrue( + checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, + float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, + uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, + stringValue1)); + + assertTrue( + checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, + float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, + uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, + stringValue2)); + } + + @Test + public final void testPubSubNested() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.Nested.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.Nested.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.Nested msg = new rcljava.msg.Nested(); + msg.setPrimitiveValues(primitives1); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + rcljava.msg.Nested value = future.get(); + assertNotEquals(null, value.getPrimitiveValues()); + + rcljava.msg.Primitives primitivesValues = value.getPrimitiveValues(); + + assertTrue( + checkPrimitives(primitivesValues, boolValue1, byteValue1, charValue1, + float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, + uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, + stringValue1)); + } + + @Test + public final void testPubSubPrimitives() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.Primitives.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.Primitives.class, + "test_topic", + new TestConsumer(future)); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(primitives1); + RCLJava.spinOnce(node); + } + + rcljava.msg.Primitives primitivesValues = future.get(); + assertNotEquals(null, primitivesValues); + + assertTrue( + checkPrimitives(primitivesValues, boolValue1, byteValue1, charValue1, + float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, + uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, + stringValue1)); + } + + @Test + public final void testPubSubStaticArrayNested() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.StaticArrayNested.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.StaticArrayNested.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.StaticArrayNested msg = new rcljava.msg.StaticArrayNested(); + msg.setPrimitiveValues(Arrays.asList(new rcljava.msg.Primitives[] {primitives1, primitives2, primitives1, primitives2})); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + rcljava.msg.StaticArrayNested value = future.get(); + assertNotEquals(null, value.getPrimitiveValues()); + + assertEquals(4, value.getPrimitiveValues().size()); + + 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); + + assertTrue( + checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, + float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, + uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, + stringValue1)); + + assertTrue( + checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, + float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, + uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, + stringValue2)); + + assertTrue( + checkPrimitives(primitivesValue3, boolValue1, byteValue1, charValue1, + float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, + uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, + stringValue1)); + + assertTrue( + checkPrimitives(primitivesValue4, boolValue2, byteValue2, charValue2, + float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, + uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, + stringValue2)); + } + + @Test + public final void testPubSubStaticArrayPrimitives() throws Exception { + Publisher publisher = node + .createPublisher( + rcljava.msg.StaticArrayPrimitives.class, "test_topic"); + + RCLFuture future = + new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription( + rcljava.msg.StaticArrayPrimitives.class, + "test_topic", + new TestConsumer(future)); + + rcljava.msg.StaticArrayPrimitives msg = new rcljava.msg.StaticArrayPrimitives(); + + List boolValues = Arrays.asList(new Boolean[] {true, false, true}); + List byteValues = Arrays.asList(new Byte[] {123, 42, 24}); + List charValues = Arrays.asList(new Character[] {'\u0012', '\u0021', '\u0008'}); + List float32Values = Arrays.asList(new Float[] {12.34f, 13.34f, 14.34f}); + List float64Values = Arrays.asList(new Double[] {43.21, 54.21, 65.21}); + List int8Values = Arrays.asList(new Byte[] {-12, -13, -14}); + List uint8Values = Arrays.asList(new Byte[] {34, 35, 36}); + List int16Values = Arrays.asList(new Short[] {-1234, -1235, -1236}); + List uint16Values = Arrays.asList(new Short[] {4321, 4322, 4323}); + List int32Values = Arrays.asList(new Integer[] {-75536, -75537, -75532}); + List uint32Values = Arrays.asList(new Integer[] {85536, 85537, 85535}); + List int64Values = Arrays.asList(new Long[] {-5294967296l, -5294967297l, -5294967295l}); + List uint64Values = Arrays.asList(new Long[] {6294967296l, 6294967297l, 6294967296l}); + List stringValues = Arrays.asList(new String[] {"hello world", "bye world", "hey world"}); + + msg.setBoolValues(boolValues); + msg.setByteValues(byteValues); + msg.setCharValues(charValues); + msg.setFloat32Values(float32Values); + msg.setFloat64Values(float64Values); + msg.setInt8Values(int8Values); + msg.setUint8Values(uint8Values); + msg.setInt16Values(int16Values); + msg.setUint16Values(uint16Values); + msg.setInt32Values(int32Values); + msg.setUint32Values(uint32Values); + msg.setInt64Values(int64Values); + msg.setUint64Values(uint64Values); + msg.setStringValues(stringValues); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + 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()); + } + + @Test + public final void testPubUInt32() throws Exception { + Publisher publisher = node + .createPublisher(rcljava.msg.UInt32.class, + "test_topic"); + + RCLFuture future = new RCLFuture(new WeakReference(node)); + + Subscription subscription = node + .createSubscription(rcljava.msg.UInt32.class, + "test_topic", new TestConsumer(future)); + + rcljava.msg.UInt32 msg = new rcljava.msg.UInt32(); + msg.setData(12345); + + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } + + rcljava.msg.UInt32 value = future.get(); + assertEquals(12345, value.getData()); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/NodeTest.java b/rcljava/src/test/java/org/ros2/rcljava/NodeTest.java index d25fca1d..a71fe9cd 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/NodeTest.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,742 +13,363 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; -import static org.junit.Assert.assertTrue; -import org.junit.Before; +import org.apache.log4j.BasicConfigurator; + +import org.junit.Assert; import org.junit.BeforeClass; import org.junit.Test; -import java.lang.ref.WeakReference; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.service.Client; +import org.ros2.rcljava.node.service.RCLFuture; +import org.ros2.rcljava.node.service.RMWRequestId; +import org.ros2.rcljava.node.service.Service; +import org.ros2.rcljava.node.service.TriConsumer; +import org.ros2.rcljava.node.topic.Consumer; +import org.ros2.rcljava.node.topic.Publisher; +import org.ros2.rcljava.node.topic.Subscription; +import org.ros2.rcljava.qos.QoSProfile; -import java.util.Arrays; -import java.util.List; +import java.lang.ref.WeakReference; +import java.util.HashMap; +import java.util.Map.Entry; -import org.ros2.rcljava.Node; import org.ros2.rcljava.RCLJava; public class NodeTest { - private Node node; - private rcljava.msg.Primitives primitives1; - private rcljava.msg.Primitives primitives2; - - private boolean boolValue1, boolValue2; - private byte byteValue1, byteValue2; - private char charValue1, charValue2; - private float float32Value1, float32Value2; - private double float64Value1, float64Value2; - private byte int8Value1, int8Value2; - private byte uint8Value1, uint8Value2; - private short int16Value1, int16Value2; - private short uint16Value1, uint16Value2; - private int int32Value1, int32Value2; - private int uint32Value1, uint32Value2; - private long int64Value1, int64Value2; - private long uint64Value1, uint64Value2; - private String stringValue1, stringValue2; - - boolean checkPrimitives(rcljava.msg.Primitives primitives, - boolean booleanValue, byte byteValue, char charValue, float float32Value, - double float64Value, byte int8Value, byte uint8Value, short int16Value, - short uint16Value, int int32Value, int uint32Value, long int64Value, - long uint64Value, String stringValue) { - - boolean result = true; - result = result && (primitives.getBoolValue() == booleanValue); - result = result && (primitives.getByteValue() == byteValue); - result = result && (primitives.getCharValue() == charValue); - result = result && (primitives.getFloat32Value() == float32Value); - result = result && (primitives.getFloat64Value() == float64Value); - result = result && (primitives.getInt8Value() == int8Value); - result = result && (primitives.getUint8Value() == uint8Value); - result = result && (primitives.getInt16Value() == int16Value); - result = result && (primitives.getUint16Value() == uint16Value); - result = result && (primitives.getInt32Value() == int32Value); - result = result && (primitives.getUint32Value() == uint32Value); - result = result && (primitives.getInt64Value() == int64Value); - result = result && (primitives.getUint64Value() == uint64Value); - result = result && (primitives.getStringValue().equals(stringValue)); - - return result; - } - - @BeforeClass - public static void setupOnce() throws Exception { - RCLJava.rclJavaInit(); - org.apache.log4j.BasicConfigurator.configure(); - } - - public class TestConsumer implements Consumer { - private final RCLFuture future; - - TestConsumer(final RCLFuture future) { - this.future = future; - } + public class TestConsumer implements Consumer { + private final RCLFuture future; - public final void accept(final T msg) { - if(!this.future.isDone()) { - this.future.set(msg); - } - } - } - - @Before - public void setUp() { - node = RCLJava.createNode("test_node"); - - primitives1 = new rcljava.msg.Primitives(); - primitives2 = new rcljava.msg.Primitives(); - - boolValue1 = true; - byteValue1 = (byte)123; - charValue1 = '\u0012'; - float32Value1 = 12.34f; - float64Value1 = 43.21; - int8Value1 = (byte)-12; - uint8Value1 = (byte)34; - int16Value1 = (short)-1234; - uint16Value1 = (short)4321; - int32Value1 = -75536; - uint32Value1 = 85536; - int64Value1 = -5294967296l; - uint64Value1 = 6294967296l; - stringValue1 = "hello world"; - - primitives1.setBoolValue(boolValue1); - primitives1.setByteValue(byteValue1); - primitives1.setCharValue(charValue1); - primitives1.setFloat32Value(float32Value1); - primitives1.setFloat64Value(float64Value1); - primitives1.setInt8Value(int8Value1); - primitives1.setUint8Value(uint8Value1); - primitives1.setInt16Value(int16Value1); - primitives1.setUint16Value(uint16Value1); - primitives1.setInt32Value(int32Value1); - primitives1.setUint32Value(uint32Value1); - primitives1.setInt64Value(int64Value1); - primitives1.setUint64Value(uint64Value1); - primitives1.setStringValue(stringValue1); - - boolValue2 = false; - byteValue2 = (byte)42; - charValue2 = '\u0021'; - float32Value2 = 13.34f; - float64Value2 = 44.21; - int8Value2 = (byte)-13; - uint8Value2 = (byte)35; - int16Value2 = (short)-1235; - uint16Value2 = (short)4321; - int32Value2 = -75536; - uint32Value2 = 85536; - int64Value2 = -5294967296l; - uint64Value2 = 6294967296l; - stringValue2 = "bye world"; - - primitives2.setBoolValue(boolValue2); - primitives2.setByteValue(byteValue2); - primitives2.setCharValue(charValue2); - primitives2.setFloat32Value(float32Value2); - primitives2.setFloat64Value(float64Value2); - primitives2.setInt8Value(int8Value2); - primitives2.setUint8Value(uint8Value2); - primitives2.setInt16Value(int16Value2); - primitives2.setUint16Value(uint16Value2); - primitives2.setInt32Value(int32Value2); - primitives2.setUint32Value(uint32Value2); - primitives2.setInt64Value(int64Value2); - primitives2.setUint64Value(uint64Value2); - primitives2.setStringValue(stringValue2); - } - - @Test - public final void testCreate() { - assertNotEquals(0, node.getNodeHandle()); - } - - @Test - public final void testPubSubStdString() throws Exception { - Publisher publisher = node - .createPublisher(std_msgs.msg.String.class, - "test_topic"); - - RCLFuture future = new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription(std_msgs.msg.String.class, - "test_topic", new TestConsumer(future)); - - std_msgs.msg.String msg = new std_msgs.msg.String(); - msg.setData("Hello"); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); - } + TestConsumer(final RCLFuture future) { + this.future = future; + } - std_msgs.msg.String value = future.get(); - assertEquals("Hello", value.getData()); - } - - @Test - public final void testPubSubBoundedArrayNested() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.BoundedArrayNested.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.BoundedArrayNested.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.BoundedArrayNested msg = new rcljava.msg.BoundedArrayNested(); - msg.setPrimitiveValues(Arrays.asList(new rcljava.msg.Primitives[] {primitives1, primitives2})); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + public final void accept(final std_msgs.msg.String msg) { + if (!this.future.isDone()) { + this.future.set(msg); + } + } } - 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); - - assertTrue( - checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, - float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, - uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, - stringValue1)); - - assertTrue( - checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, - float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, - uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, - stringValue2)); - } - - @Test - public final void testPubSubBoundedArrayPrimitives() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.BoundedArrayPrimitives.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.BoundedArrayPrimitives.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.BoundedArrayPrimitives msg = new rcljava.msg.BoundedArrayPrimitives(); - - List boolValues = Arrays.asList(new Boolean[] {true, false, true}); - List byteValues = Arrays.asList(new Byte[] {123, 42}); - List charValues = Arrays.asList(new Character[] {'\u0012', '\u0021'}); - List float32Values = Arrays.asList(new Float[] {12.34f, 13.34f}); - List float64Values = Arrays.asList(new Double[] {43.21, 44.21}); - List int8Values = Arrays.asList(new Byte[] {-12, -13}); - List uint8Values = Arrays.asList(new Byte[] {34, 35}); - List int16Values = Arrays.asList(new Short[] {-1234, -1235}); - List uint16Values = Arrays.asList(new Short[] {4321, 4322}); - List int32Values = Arrays.asList(new Integer[] {-75536, -75537}); - List uint32Values = Arrays.asList(new Integer[] {85536, 85537}); - List int64Values = Arrays.asList(new Long[] {-5294967296l, -5294967297l}); - List uint64Values = Arrays.asList(new Long[] {6294967296l, 6294967297l}); - List stringValues = Arrays.asList(new String[] {"hello world", "bye world"}); - - msg.setBoolValues(boolValues); - msg.setByteValues(byteValues); - msg.setCharValues(charValues); - msg.setFloat32Values(float32Values); - msg.setFloat64Values(float64Values); - msg.setInt8Values(int8Values); - msg.setUint8Values(uint8Values); - msg.setInt16Values(int16Values); - msg.setUint16Values(uint16Values); - msg.setInt32Values(int32Values); - msg.setUint32Values(uint32Values); - msg.setInt64Values(int64Values); - msg.setUint64Values(uint64Values); - msg.setStringValues(stringValues); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @BeforeClass + public static void beforeClass() { + BasicConfigurator.resetConfiguration(); + BasicConfigurator.configure(); } - 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()); - } - - @Test - public final void testPubSubBuiltins() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.Builtins.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.Builtins.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.Builtins msg = new rcljava.msg.Builtins(); - - builtin_interfaces.msg.Duration duration = new builtin_interfaces.msg.Duration(); - duration.setSec(1234); - duration.setNanosec(4567); - - builtin_interfaces.msg.Time time = new builtin_interfaces.msg.Time(); - time.setSec(4321); - time.setNanosec(7654); - - msg.setDurationValue(duration); - msg.setTimeValue(time); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @Test + public final void testCreate() { + boolean test = true; + Node node = null; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("test_node"); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertNotEquals(0, node.getNodeHandle()); } - rcljava.msg.Builtins value = future.get(); - builtin_interfaces.msg.Duration durationValue = value.getDurationValue(); - builtin_interfaces.msg.Time timeValue = value.getTimeValue(); - - assertEquals(1234, durationValue.getSec()); - assertEquals(4567, durationValue.getNanosec()); - - assertEquals(4321, timeValue.getSec()); - assertEquals(7654, timeValue.getNanosec()); - } - - @Test - public final void testPubSubDynamicArrayNested() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.DynamicArrayNested.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.DynamicArrayNested.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.DynamicArrayNested msg = new rcljava.msg.DynamicArrayNested(); - msg.setPrimitiveValues(Arrays.asList(new rcljava.msg.Primitives[] {primitives1, primitives2})); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @Test + public void testDestroyNode() { + boolean test = true; + Node node = null; + + RCLJava.rclJavaInit(); + node = RCLJava.createNode("test_node"); + try { + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertEquals("Bad result", node, node); } - 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); - - assertTrue( - checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, - float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, - uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, - stringValue1)); - - assertTrue( - checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, - float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, - uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, - stringValue2)); - } - - @Test - public final void testPubSubDynamicArrayPrimitives() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.DynamicArrayPrimitives.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.DynamicArrayPrimitives.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.DynamicArrayPrimitives msg = new rcljava.msg.DynamicArrayPrimitives(); - - List boolValues = Arrays.asList(new Boolean[] {true, false, true}); - List byteValues = Arrays.asList(new Byte[] {123, 42}); - List charValues = Arrays.asList(new Character[] {'\u0012', '\u0021'}); - List float32Values = Arrays.asList(new Float[] {12.34f, 13.34f}); - List float64Values = Arrays.asList(new Double[] {43.21, 44.21}); - List int8Values = Arrays.asList(new Byte[] {-12, -13}); - List uint8Values = Arrays.asList(new Byte[] {34, 35}); - List int16Values = Arrays.asList(new Short[] {-1234, -1235}); - List uint16Values = Arrays.asList(new Short[] {4321, 4322}); - List int32Values = Arrays.asList(new Integer[] {-75536, -75537}); - List uint32Values = Arrays.asList(new Integer[] {85536, 85537}); - List int64Values = Arrays.asList(new Long[] {-5294967296l, -5294967297l}); - List uint64Values = Arrays.asList(new Long[] {6294967296l, 6294967297l}); - List stringValues = Arrays.asList(new String[] {"hello world", "bye world"}); - - msg.setBoolValues(boolValues); - msg.setByteValues(byteValues); - msg.setCharValues(charValues); - msg.setFloat32Values(float32Values); - msg.setFloat64Values(float64Values); - msg.setInt8Values(int8Values); - msg.setUint8Values(uint8Values); - msg.setInt16Values(int16Values); - msg.setUint16Values(uint16Values); - msg.setInt32Values(int32Values); - msg.setUint32Values(uint32Values); - msg.setInt64Values(int64Values); - msg.setUint64Values(uint64Values); - msg.setStringValues(stringValues); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @Test + public void testGetNodeName() { + boolean test = true; + Node node = null; + String nodeName = null; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testNodeName"); + nodeName = node.getName(); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertEquals("Bad result", "testNodeName", nodeName); } - 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()); - } - - @Test - public final void testPubSubEmpty() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.Empty.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.Empty.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.Empty msg = new rcljava.msg.Empty(); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); - } + @Test + public final void testPubSub() throws Exception { + RCLJava.rclJavaInit(); + Node node = RCLJava.createNode("test_node"); + assertNotEquals(0, node.getNodeHandle()); - rcljava.msg.Empty value = future.get(); - assertNotEquals(null, value); - } + Publisher publisher = node.createPublisher(std_msgs.msg.String.class, + "test_topic"); - @Test - public final void testPubSubFieldsWithSameType() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.FieldsWithSameType.class, "test_topic"); + RCLFuture future = new RCLFuture(new WeakReference(node)); - RCLFuture future = - new RCLFuture(new WeakReference(node)); + Subscription subscription = node.createSubscription( + std_msgs.msg.String.class, "test_topic", new TestConsumer(future)); - Subscription subscription = node - .createSubscription( - rcljava.msg.FieldsWithSameType.class, - "test_topic", - new TestConsumer(future)); + std_msgs.msg.String msg = new std_msgs.msg.String(); + msg.setData("Hello"); - rcljava.msg.FieldsWithSameType msg = new rcljava.msg.FieldsWithSameType(); + while (RCLJava.ok() && !future.isDone()) { + publisher.publish(msg); + RCLJava.spinOnce(node); + } - msg.setPrimitiveValues1(primitives1); - msg.setPrimitiveValues2(primitives2); + std_msgs.msg.String value = future.get(); + assertEquals("Hello", value.getData()); - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + subscription.dispose(); + node.dispose(); + RCLJava.shutdown(); } - rcljava.msg.FieldsWithSameType value = future.get(); - assertNotEquals(null, value.getPrimitiveValues1()); - assertNotEquals(null, value.getPrimitiveValues2()); - - rcljava.msg.Primitives primitivesValue1 = value.getPrimitiveValues1(); - rcljava.msg.Primitives primitivesValue2 = value.getPrimitiveValues2(); - - assertTrue( - checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, - float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, - uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, - stringValue1)); - - assertTrue( - checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, - float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, - uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, - stringValue2)); - } - - @Test - public final void testPubSubNested() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.Nested.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.Nested.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.Nested msg = new rcljava.msg.Nested(); - msg.setPrimitiveValues(primitives1); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @Test + public void testPublisher() { + boolean test = true; + Node node = null; + Publisher pub = null; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testPublisher"); +// RCLFuture future = new RCLFuture(new WeakReference(node)); + pub = node.createPublisher( + std_msgs.msg.String.class, + "testChannel", + QoSProfile.DEFAULT); + +// std_msgs.msg.String msg = new std_msgs.msg.String(); +// msg.setData("Hello"); +// +// while (RCLJava.ok() && !future.isDone()) { +// pub.publish(msg); +// RCLJava.spinOnce(node); +// } + + pub.dispose(); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertNotNull("Bad result", pub); } - rcljava.msg.Nested value = future.get(); - assertNotEquals(null, value.getPrimitiveValues()); - - rcljava.msg.Primitives primitivesValues = value.getPrimitiveValues(); - - assertTrue( - checkPrimitives(primitivesValues, boolValue1, byteValue1, charValue1, - float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, - uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, - stringValue1)); - } - - @Test - public final void testPubSubPrimitives() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.Primitives.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); + @Test + public void testSubscription() { + boolean test = true; + Node node = null; + Subscription sub = null; + + Consumer callback = new Consumer() { + @Override + public void accept(std_msgs.msg.String msg) { } + }; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testSubscription"); + sub = node.createSubscription( + std_msgs.msg.String.class, + "testChannel", + callback, + QoSProfile.DEFAULT); + + sub.dispose(); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertNotNull("Bad result", sub); + } - Subscription subscription = node - .createSubscription( - rcljava.msg.Primitives.class, - "test_topic", - new TestConsumer(future)); + @Test + public void testClient() { + boolean test = true; + Node node = null; + Client clt = null; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testClient"); + clt = node.createClient( + rcl_interfaces.srv.GetParameters.class, + "testChannel", + QoSProfile.DEFAULT); + + clt.dispose(); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertNotNull("Bad result", clt); + } - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(primitives1); - RCLJava.spinOnce(node); + @Test + public void testService() { + boolean test = true; + Node node = null; + Service srv = null; + + TriConsumer callback = + new TriConsumer() { + @Override + public void accept(RMWRequestId header, rcl_interfaces.srv.GetParameters_Request request, rcl_interfaces.srv.GetParameters_Response response) { } + }; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testSubscription"); + srv = node.createService( + rcl_interfaces.srv.GetParameters.class, + "testChannel", + callback, + QoSProfile.DEFAULT); + + srv.dispose(); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertNotNull("Bad result", srv); } - rcljava.msg.Primitives primitivesValues = future.get(); - assertNotEquals(null, primitivesValues); - - assertTrue( - checkPrimitives(primitivesValues, boolValue1, byteValue1, charValue1, - float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, - uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, - stringValue1)); - } - - @Test - public final void testPubSubStaticArrayNested() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.StaticArrayNested.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.StaticArrayNested.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.StaticArrayNested msg = new rcljava.msg.StaticArrayNested(); - msg.setPrimitiveValues(Arrays.asList(new rcljava.msg.Primitives[] {primitives1, primitives2, primitives1, primitives2})); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @Test + public void testGraphPublisherCount() { + boolean test = true; + int count = -2; + Node node = null; +// Publisher pub = null; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testPublisher"); + + count = node.countPublishers("testChannel"); + Assert.assertEquals("Bad result", 0, count); + // TODO +// pub = node.createPublisher( +// std_msgs.msg.String.class, +// "testChannel", +// QoSProfile.PROFILE_DEFAULT); +// count = node.countPublishers("testChannel"); +// Assert.assertEquals("Bad result", 1, count); +// pub.dispose(); + count = node.countPublishers("testChannel"); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertEquals("Bad result", 0, count); } - rcljava.msg.StaticArrayNested value = future.get(); - assertNotEquals(null, value.getPrimitiveValues()); - - assertEquals(4, value.getPrimitiveValues().size()); - - 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); - - assertTrue( - checkPrimitives(primitivesValue1, boolValue1, byteValue1, charValue1, - float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, - uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, - stringValue1)); - - assertTrue( - checkPrimitives(primitivesValue2, boolValue2, byteValue2, charValue2, - float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, - uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, - stringValue2)); - - assertTrue( - checkPrimitives(primitivesValue3, boolValue1, byteValue1, charValue1, - float32Value1, float64Value1, int8Value1, uint8Value1, int16Value1, - uint16Value1, int32Value1, uint32Value1, int64Value1, uint64Value1, - stringValue1)); - - assertTrue( - checkPrimitives(primitivesValue4, boolValue2, byteValue2, charValue2, - float32Value2, float64Value2, int8Value2, uint8Value2, int16Value2, - uint16Value2, int32Value2, uint32Value2, int64Value2, uint64Value2, - stringValue2)); - } - - @Test - public final void testPubSubStaticArrayPrimitives() throws Exception { - Publisher publisher = node - .createPublisher( - rcljava.msg.StaticArrayPrimitives.class, "test_topic"); - - RCLFuture future = - new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription( - rcljava.msg.StaticArrayPrimitives.class, - "test_topic", - new TestConsumer(future)); - - rcljava.msg.StaticArrayPrimitives msg = new rcljava.msg.StaticArrayPrimitives(); - - List boolValues = Arrays.asList(new Boolean[] {true, false, true}); - List byteValues = Arrays.asList(new Byte[] {123, 42, 24}); - List charValues = Arrays.asList(new Character[] {'\u0012', '\u0021', '\u0008'}); - List float32Values = Arrays.asList(new Float[] {12.34f, 13.34f, 14.34f}); - List float64Values = Arrays.asList(new Double[] {43.21, 54.21, 65.21}); - List int8Values = Arrays.asList(new Byte[] {-12, -13, -14}); - List uint8Values = Arrays.asList(new Byte[] {34, 35, 36}); - List int16Values = Arrays.asList(new Short[] {-1234, -1235, -1236}); - List uint16Values = Arrays.asList(new Short[] {4321, 4322, 4323}); - List int32Values = Arrays.asList(new Integer[] {-75536, -75537, -75532}); - List uint32Values = Arrays.asList(new Integer[] {85536, 85537, 85535}); - List int64Values = Arrays.asList(new Long[] {-5294967296l, -5294967297l, -5294967295l}); - List uint64Values = Arrays.asList(new Long[] {6294967296l, 6294967297l, 6294967296l}); - List stringValues = Arrays.asList(new String[] {"hello world", "bye world", "hey world"}); - - msg.setBoolValues(boolValues); - msg.setByteValues(byteValues); - msg.setCharValues(charValues); - msg.setFloat32Values(float32Values); - msg.setFloat64Values(float64Values); - msg.setInt8Values(int8Values); - msg.setUint8Values(uint8Values); - msg.setInt16Values(int16Values); - msg.setUint16Values(uint16Values); - msg.setInt32Values(int32Values); - msg.setUint32Values(uint32Values); - msg.setInt64Values(int64Values); - msg.setUint64Values(uint64Values); - msg.setStringValues(stringValues); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @Test + public void testGraphSubscriptionCount() { + boolean test = true; + int count = -2; + Node node = null; +// Subscription sub = null; + +// Consumer callback = new Consumer() { +// @Override +// public void accept(std_msgs.msg.String msg) { } +// }; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testSubscription"); + count = node.countPublishers("testChannel"); + Assert.assertEquals("Bad result", 0, count); + // TODO +// sub = node.createSubscription( +// std_msgs.msg.String.class, +// "testChannel", +// callback, +// QoSProfile.PROFILE_DEFAULT); +// count = node.countPublishers("testChannel"); +// Assert.assertEquals("Bad result", 1, count); +// sub.dispose(); + count = node.countPublishers("testChannel"); + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertEquals("Bad result", 0, count); } - 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()); - } - - @Test - public final void testPubUInt32() throws Exception { - Publisher publisher = node - .createPublisher(rcljava.msg.UInt32.class, - "test_topic"); - - RCLFuture future = new RCLFuture(new WeakReference(node)); - - Subscription subscription = node - .createSubscription(rcljava.msg.UInt32.class, - "test_topic", new TestConsumer(future)); - - rcljava.msg.UInt32 msg = new rcljava.msg.UInt32(); - msg.setData(12345); - - while (RCLJava.ok() && !future.isDone()) { - publisher.publish(msg); - RCLJava.spinOnce(node); + @Test + public void testGraphGetTopics() { + boolean test = true; + Node node = null; + HashMap topics = null; + String fqnNode = null; + + RCLJava.rclJavaInit(); + try { + node = RCLJava.createNode("testSubscription"); + topics = node.getTopicNamesAndTypes(); + fqnNode = "/" + node.getName(); + + node.dispose(); + } catch (Exception e) { + test = false; + } + RCLJava.shutdown(); + + int i = 0; + for (Entry topic : topics.entrySet()) { + if (topic.getKey().startsWith(fqnNode)) { + ++i; + } + } + + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertEquals("Bad result", 13, i); } - rcljava.msg.UInt32 value = future.get(); - assertEquals(12345, value.getData()); - } + //TODO Test Parameters } diff --git a/rcljava/src/test/java/org/ros2/rcljava/PublisherTest.java b/rcljava/src/test/java/org/ros2/rcljava/PublisherTest.java index 9fbe7b14..09217c8b 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/PublisherTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/PublisherTest.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,19 +19,33 @@ import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; +import org.apache.log4j.BasicConfigurator; +import org.junit.BeforeClass; import org.junit.Test; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.topic.Publisher; public class PublisherTest { - @Test - public final void testCreate() { - RCLJava.rclJavaInit(); - Node node = RCLJava.createNode("test_node"); - Publisher publisher = node - .createPublisher(std_msgs.msg.String.class, - "test_topic"); - assertEquals(node.getNodeHandle(), publisher.getNodeHandle()); - assertNotEquals(0, publisher.getNodeHandle()); - assertNotEquals(0, publisher.getPublisherHandle()); - } + @BeforeClass + public static void beforeClass() { + BasicConfigurator.resetConfiguration(); + BasicConfigurator.configure(); + } + + @Test + public final void testCreate() { + RCLJava.rclJavaInit(); + Node node = RCLJava.createNode("test_node"); + Publisher publisher = node.createPublisher(std_msgs.msg.String.class, + "test_topic"); + + assertEquals(node.getNodeHandle(), publisher.getNode().getNodeHandle()); + assertNotEquals(0, publisher.getNode().getNodeHandle()); + assertNotEquals(0, publisher.getPublisherHandle()); + + publisher.dispose(); + node.dispose(); + RCLJava.shutdown(); + } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/RCLJavaTest.java b/rcljava/src/test/java/org/ros2/rcljava/RCLJavaTest.java index a6b2e140..382fa75e 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/RCLJavaTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/RCLJavaTest.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -12,28 +13,153 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava; import static org.junit.Assert.assertEquals; +import org.apache.log4j.BasicConfigurator; +import org.junit.Assert; +import org.junit.BeforeClass; import org.junit.Test; +import org.ros2.rcljava.exception.NotInitializedException; +import org.ros2.rcljava.node.Node; public class RCLJavaTest { - @Test - public final void testInit() { - assertEquals(false, RCLJava.isInitialized()); - RCLJava.rclJavaInit(); - assertEquals(true, RCLJava.isInitialized()); - } - - @Test - public final void testOk() { - RCLJava.rclJavaInit(); - assertEquals(true, RCLJava.ok()); - RCLJava.shutdown(); - assertEquals(false, RCLJava.ok()); - } + @BeforeClass + public static void beforeClass() { + BasicConfigurator.resetConfiguration(); + BasicConfigurator.configure(); + } + + @Test + public void testInit() { + boolean test = true; + + try { + Assert.assertEquals(false, RCLJava.isInitialized()); + RCLJava.rclJavaInit(); + Assert.assertEquals(true, RCLJava.isInitialized()); + } catch (Exception e) { + test = false; + } + + RCLJava.shutdown(); + Assert.assertTrue("failed to initialize rclJava", test); + } + + @Test + public void testInitShutdown() { + boolean test = true; + + try { + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + } catch (Exception e) { + test = false; + } + + Assert.assertTrue("failed to shutdown rclJava", test); + } + + @Test + public void testInitShutdownSequence() { + boolean test = true; + + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + try { + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + } catch (Exception e) { + test = false; + } + + Assert.assertTrue("failed to initialize rclJava after shutdown", test); + } + + @Test + public void testInitDouble() { + boolean test = false; + + RCLJava.rclJavaInit(); + try { + RCLJava.rclJavaInit(); + } catch (Exception e) { + test = true; + } + + RCLJava.shutdown(); + Assert.assertTrue("Expected Runtime error when initializing rclJava twice", test); + } + + @Test + public void testShutdownDouble() { + boolean test = false; + + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + try { + RCLJava.shutdown(); + } catch (Exception e) { + test = true; + } + + Assert.assertTrue("Expected Runtime error when shutting down rclJava twice", test); + } + + @Test + public void testCreateNode() { + boolean test = true; + Node node = null; + + RCLJava.rclJavaInit(); + + try { + node = RCLJava.createNode("testNode"); + node.dispose(); + } catch (Exception e) { + test = false; + } + + RCLJava.shutdown(); + Assert.assertTrue("Expected Runtime error.", test); + Assert.assertEquals("Bad result", node, node); + } + + @Test + public void testOk() { + boolean test = true; + + RCLJava.rclJavaInit(); + + try { + test = RCLJava.ok(); + Assert.assertEquals(true, RCLJava.ok()); + } catch (Exception e) { + test = false; + } + + try { + RCLJava.shutdown(); + assertEquals(false, RCLJava.ok()); + test = false; + } catch (Exception e) { } + + Assert.assertTrue("Expected Runtime error.", test); + } + + @Test + public void testNotInitializedException() { + boolean test = false; + + try { + RCLJava.createNode("testNode"); + } catch (NotInitializedException e) { + test = true; + } + + Assert.assertTrue("failed not initialized exception !", test); + } } diff --git a/rcljava/src/test/java/org/ros2/rcljava/RmwTest.java b/rcljava/src/test/java/org/ros2/rcljava/RmwTest.java new file mode 100644 index 00000000..ebbd0e28 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/RmwTest.java @@ -0,0 +1,156 @@ +/* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard + * + * 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; + +import org.apache.log4j.BasicConfigurator; +import org.junit.Assert; +import org.junit.BeforeClass; +import org.junit.Ignore; +import org.junit.Test; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.exception.ImplementationAlreadyImportedException; +import org.ros2.rcljava.exception.NoImplementationAvailableException; + +/** + * + */ +public class RmwTest { + + @BeforeClass + public static void beforeClass() { + BasicConfigurator.resetConfiguration(); + BasicConfigurator.configure(); + } + + @Test + public void testRmwFastrtps() { + boolean test = true; + + try { + RCLJava.setRMWImplementation("rmw_fastrtps_cpp"); + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + } catch (Exception e) { + test = false; + } finally { + try { + RCLJava.setRMWImplementation(null); + } catch (Exception e) { } + } + + Assert.assertTrue("failed to initialize rclJava with Fastrtps", test); + } + + @Ignore + @Test + public void testRmwOpensplice() { + boolean test = true; + + try { + RCLJava.setRMWImplementation("rmw_opensplice_cpp"); + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + } catch (Exception e) { + test = false; + } finally { + try { + RCLJava.setRMWImplementation(null); + } catch (Exception e) { } + } + + Assert.assertTrue("failed to initialize rclJava with Opensplice", test); + } + + @Ignore + @Test + public void testRmwConnext() { + boolean test = true; + + try { + RCLJava.setRMWImplementation("rmw_connext_cpp"); + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + } catch (Exception e) { + test = false; + } finally { + try { + RCLJava.setRMWImplementation(null); + } catch (Exception e) { } + } + + Assert.assertTrue("failed to initialize rclJava with Connext", test); + } + + @Ignore + @Test + public void testRmwConnextDynamic() { + boolean test = true; + + try { + RCLJava.setRMWImplementation("rmw_connext_dynamic_cpp"); + RCLJava.rclJavaInit(); + RCLJava.shutdown(); + } catch (Exception e) { + test = false; + } finally { + try { + RCLJava.setRMWImplementation(null); + } catch (Exception e) { } + } + + Assert.assertTrue("failed to initialize rclJava with Connext Dynamic", test); + } + + @Test + public void testNoImplementationAvailableException() { + boolean test = false; + + try { + RCLJava.setRMWImplementation("foo"); + } catch (NoImplementationAvailableException e) { + test = true; + } catch (ImplementationAlreadyImportedException e) { + test = false; + } finally { + try { + RCLJava.setRMWImplementation(null); + } catch (Exception e) { } + } + + Assert.assertTrue("failed not implementation available exception !", test); + } + + @Test + public void testImplementationAlreadyImportedException() { + boolean test = false; + + try { + RCLJava.setRMWImplementation("rmw_fastrtps_cpp"); + RCLJava.setRMWImplementation("rmw_fastrtps_cpp"); + } catch (ImplementationAlreadyImportedException e) { + test = true; + } catch (NoImplementationAvailableException e) { + test = false; + } finally { + try { + RCLJava.setRMWImplementation(null); + } catch (Exception e) { } + } + + Assert.assertTrue("failed implementation already imported exception !", test); + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/SubscriptionTest.java b/rcljava/src/test/java/org/ros2/rcljava/SubscriptionTest.java index 92aab66e..5eae93a3 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/SubscriptionTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/SubscriptionTest.java @@ -1,4 +1,5 @@ /* Copyright 2016 Esteve Fernandez + * Copyright 2016 Mickael Gaillard * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,23 +19,37 @@ import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; +import org.apache.log4j.BasicConfigurator; +import org.junit.BeforeClass; import org.junit.Test; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.node.topic.Consumer; +import org.ros2.rcljava.node.topic.Subscription; public class SubscriptionTest { - @Test - public final void testCreate() { - 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) { - } - } - ); - assertEquals(node.getNodeHandle(), subscription.getNodeHandle()); - assertNotEquals(0, subscription.getNodeHandle()); - assertNotEquals(0, subscription.getSubscriptionHandle()); - } + @BeforeClass + public static void beforeClass() { + BasicConfigurator.resetConfiguration(); + BasicConfigurator.configure(); + } + + @Test + public final void testCreate() { + 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) { + } + }); + + assertEquals(node.getNodeHandle(), subscription.getNode().getNodeHandle()); + assertNotEquals(0, subscription.getNode().getNodeHandle()); + assertNotEquals(0, subscription.getSubscriptionHandle()); + + subscription.dispose(); + node.dispose(); + RCLJava.shutdown(); + } } diff --git a/rcljava_common/CMakeLists.txt b/rcljava_common/CMakeLists.txt index 80990d74..8961580d 100644 --- a/rcljava_common/CMakeLists.txt +++ b/rcljava_common/CMakeLists.txt @@ -27,6 +27,8 @@ set(CMAKE_JAVA_COMPILE_FLAGS "-source" "1.6" "-target" "1.6") set(${PROJECT_NAME}_java_sources "src/main/java/org/ros2/rcljava/common/RCLJavaProxy.java" + "src/main/java/org/ros2/rcljava/internal/message/Message.java" + "src/main/java/org/ros2/rcljava/internal/service/Service.java" ) set(${PROJECT_NAME}_cpp_sources diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/common/RCLJavaProxy.java b/rcljava_common/src/main/java/org/ros2/rcljava/common/RCLJavaProxy.java index 190094bd..a5c4c657 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/common/RCLJavaProxy.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/common/RCLJavaProxy.java @@ -12,7 +12,6 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - package org.ros2.rcljava.common; import java.lang.reflect.Method; @@ -23,40 +22,51 @@ * This prevents a circular dependency between generated messages and rcljava. */ public final class RCLJavaProxy { - /** - * Private constructor so this cannot be instantiated. - */ - private RCLJavaProxy() { } + /** + * Private constructor so this cannot be instantiated. + */ + private RCLJavaProxy() { } + + /** + * @return a pointer to the underlying typesupport via reflection. + */ + public static synchronized String getTypesupportIdentifier() { + try { + Class cls = Class.forName("org.ros2.rcljava.RCLJava"); + Method meth = cls.getDeclaredMethod("getTypesupportIdentifier", (Class []) null); + Object obj = meth.invoke(null); + return (String) obj; + } catch(Exception e) { + // Just return null if we can't find the typesupport identifier + return null; + } + } - /** - * @return a pointer to the underlying typesupport via reflection. - */ - public static synchronized String getTypesupportIdentifier() { - try { - Class cls = Class.forName("org.ros2.rcljava.RCLJava"); - Method meth = cls.getDeclaredMethod( - "getTypesupportIdentifier", (Class[]) null); - Object obj = meth.invoke(null, (Class[]) null); - return (String) obj; - } catch (Exception exc) { - // Just return null if we can't find the typesupport identifier - return null; + /** + * @return a pointer to the underlying typesupport via reflection. + */ + public static synchronized String getRMWIdentifier() { + try { + Class cls = Class.forName("org.ros2.rcljava.RCLJava"); + Method meth = cls.getDeclaredMethod("getRMWIdentifier", (Class []) null); + Object obj = meth.invoke(null); + return (String) obj; + } catch(Exception e) { + // Just return null if we can't find the RMW identifier + return null; + } } - } - /** - * @return a pointer to the underlying typesupport via reflection. - */ - public static synchronized String getRMWIdentifier() { - try { - Class cls = Class.forName("org.ros2.rcljava.RCLJava"); - Method meth = cls.getDeclaredMethod( - "getRMWIdentifier", (Class[]) null); - Object obj = meth.invoke(null, (Class[]) null); - return (String) obj; - } catch (Exception exc) { - // Just return null if we can't find the RMW identifier - return null; + /** + * @return a pointer to the underlying load native library via reflection. + */ + public static synchronized void loadLibrary(String name) { + try { + Class cls = Class.forName("org.ros2.rcljava.RCLJava"); + Method meth = cls.getDeclaredMethod("loadLibrary", String.class); + Object obj = meth.invoke(null, name); + } catch(Exception e) { + // TODO(esteve): handle exception + } } - } } diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/internal/message/Message.java b/rcljava_common/src/main/java/org/ros2/rcljava/internal/message/Message.java new file mode 100644 index 00000000..d7f51d38 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/internal/message/Message.java @@ -0,0 +1,24 @@ +/* Copyright 2016 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.internal.message; + +/** + * Message base. + * + * @author Mickael Gaillard + */ +public interface Message { + +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/internal/service/Service.java b/rcljava_common/src/main/java/org/ros2/rcljava/internal/service/Service.java new file mode 100644 index 00000000..9bb6bd9e --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/internal/service/Service.java @@ -0,0 +1,28 @@ +/* Copyright 2016 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.internal.service; + +import org.ros2.rcljava.internal.message.Message; + +/** + * Service base. + * + * @author Mickael Gaillard + */ +public interface Service { + + public Class getRequestType(); + public Class getResponseType(); +} diff --git a/ros2_java_android.repos b/ros2_java_android.repos deleted file mode 100644 index 1c8b39a3..00000000 --- a/ros2_java_android.repos +++ /dev/null @@ -1,53 +0,0 @@ -repositories: - eProsima/Fast-CDR: - type: git - url: https://github.com/eProsima/Fast-CDR.git - version: master - eProsima/Fast-RTPS: - type: git - url: https://github.com/esteve/Fast-RTPS.git - version: cpp11-android - eProsima/ROS-RMW-Fast-RTPS-cpp: - type: git - url: https://github.com/eProsima/ROS-RMW-Fast-RTPS-cpp.git - version: master - ros2/common_interfaces: - type: git - url: https://github.com/ros2/common_interfaces.git - version: master - ros2/rcl: - type: git - url: https://github.com/ros2/rcl.git - version: master - ros2/rcl_interfaces: - type: git - url: https://github.com/ros2/rcl_interfaces.git - version: master - ros2/rmw: - type: git - url: https://github.com/ros2/rmw.git - version: master - ros2/rmw_implementation: - type: git - url: https://github.com/ros2/rmw_implementation.git - version: master - ros2/rosidl: - type: git - url: https://github.com/ros2/rosidl.git - version: master - ros2/rosidl_dds: - type: git - url: https://github.com/ros2/rosidl_dds.git - version: master - ros2_java/ros2_java: - type: git - url: https://github.com/esteve/ros2_java.git - version: master - ros2_java/ros2_android_examples: - type: git - url: https://github.com/esteve/ros2_android_examples.git - version: master - ros2_java/rosidl_typesupport: - type: git - url: https://github.com/esteve/rosidl_typesupport.git - version: android diff --git a/ros2_java_desktop.repos b/ros2_java_desktop.repos deleted file mode 100644 index 7e3181de..00000000 --- a/ros2_java_desktop.repos +++ /dev/null @@ -1,117 +0,0 @@ -repositories: - eProsima/Fast-CDR: - type: git - url: https://github.com/eProsima/Fast-CDR.git - version: master - eProsima/Fast-RTPS: - type: git - url: https://github.com/eProsima/Fast-RTPS.git - version: master - ros/class_loader: - type: git - url: https://github.com/ros/class_loader.git - version: ros2 - ros/geometry2: - type: git - url: https://github.com/ros/geometry2.git - version: ros2_alpha - ros/poco_vendor: - type: git - url: https://github.com/ros2/poco_vendor.git - version: master - ros2/common_interfaces: - type: git - url: https://github.com/ros2/common_interfaces.git - version: master - ros2/demos: - type: git - url: https://github.com/ros2/demos.git - version: master - ros2/examples: - type: git - url: https://github.com/ros2/examples.git - version: master - ros2/launch: - type: git - url: https://github.com/ros2/launch.git - version: master - ros2/rcl: - type: git - url: https://github.com/ros2/rcl.git - version: master - ros2/rcl_interfaces: - type: git - url: https://github.com/ros2/rcl_interfaces.git - version: master - ros2/rclc: - type: git - url: https://github.com/ros2/rclc.git - version: master - ros2/rclcpp: - type: git - url: https://github.com/ros2/rclcpp.git - version: master - ros2/rclpy: - type: git - url: https://github.com/ros2/rclpy.git - version: master - ros2/realtime_support: - type: git - url: https://github.com/ros2/realtime_support.git - version: master - ros2/rmw: - type: git - url: https://github.com/ros2/rmw.git - version: master - ros2/rmw_connext: - type: git - url: https://github.com/ros2/rmw_connext.git - version: master - ros2/rmw_fastrtps: - type: git - url: https://github.com/ros2/rmw_fastrtps.git - version: master - ros2/rmw_implementation: - type: git - url: https://github.com/ros2/rmw_implementation.git - version: master - ros2/rmw_opensplice: - type: git - url: https://github.com/ros2/rmw_opensplice.git - version: master - ros2/ros1_bridge: - type: git - url: https://github.com/ros2/ros1_bridge.git - version: master - ros2/rosidl: - type: git - url: https://github.com/ros2/rosidl.git - version: master - ros2/rosidl_dds: - type: git - url: https://github.com/ros2/rosidl_dds.git - version: master - ros2/system_tests: - type: git - url: https://github.com/ros2/system_tests.git - version: master - ros2/tlsf: - type: git - url: https://github.com/ros2/tlsf.git - version: master - vendor/console_bridge: - type: git - url: https://github.com/ros/console_bridge.git - version: master - ros2_java/ros2_java: - type: git - url: https://github.com/esteve/ros2_java.git - version: master - ros2_java/ros2_java_examples: - type: git - url: https://github.com/esteve/ros2_java_examples.git - version: master - ros2_java/rosidl_typesupport: - type: git - url: https://github.com/esteve/rosidl_typesupport.git - version: java diff --git a/rosidl_generator_java/CMakeLists.txt b/rosidl_generator_java/CMakeLists.txt index a4a7c9dd..64dc2844 100644 --- a/rosidl_generator_java/CMakeLists.txt +++ b/rosidl_generator_java/CMakeLists.txt @@ -27,6 +27,8 @@ include(JavaExtra) set(CMAKE_JAVA_COMPILE_FLAGS "-source" "1.6" "-target" "1.6") +set(rosidl_generator_java_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -56,8 +58,6 @@ if(BUILD_TESTING) include(cmake/register_java.cmake) include(cmake/rosidl_generator_java_get_typesupports.cmake) - set(rosidl_generator_java_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cmake") - rosidl_generator_java_extras( "${CMAKE_CURRENT_SOURCE_DIR}/bin/rosidl_generator_java" "${CMAKE_CURRENT_SOURCE_DIR}/rosidl_generator_java/__init__.py" @@ -107,5 +107,7 @@ install( ament_package( CONFIG_EXTRAS "cmake/rosidl_generator_java_get_typesupports.cmake" "cmake/register_java.cmake" + "cmake/ament_java_install_package.cmake" "rosidl_generator_java-extras.cmake.in" + "ament_cmake_package_templates-extras.cmake" ) diff --git a/rosidl_generator_java/ament_cmake_package_templates-extras.cmake b/rosidl_generator_java/ament_cmake_package_templates-extras.cmake new file mode 100644 index 00000000..1f9cdaee --- /dev/null +++ b/rosidl_generator_java/ament_cmake_package_templates-extras.cmake @@ -0,0 +1,47 @@ +# Copyright 2014 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. + +# extract information from ament_package.templates +if(NOT PYTHON_EXECUTABLE) + message(FATAL_ERROR + "ament_cmake_package_templates: variable 'PYTHON_EXECUTABLE' must not be " + "empty") +endif() + +message("INFO ament_cmake_package_templates JAVA") +# stamp script to generate CMake code +set(_generator + "${rosidl_generator_java_DIR}/package_templates/templates_2_cmake.py") +stamp("${_generator}") + +# invoke generator script +set(_generated_file + "${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_package_templates/templates.cmake") +set(_cmd + "${PYTHON_EXECUTABLE}" + "${_generator}" + "${_generated_file}" +) +execute_process( + COMMAND ${_cmd} + RESULT_VARIABLE _res +) +if(NOT _res EQUAL 0) + string(REPLACE ";" " " _cmd_str "${_cmd}") + message(FATAL_ERROR + "execute_process(${_cmd_str}) returned error code ${_res}") +endif() + +# load extracted variables into cmake +include("${_generated_file}") diff --git a/rosidl_generator_java/cmake/ament_java_install_package.cmake b/rosidl_generator_java/cmake/ament_java_install_package.cmake new file mode 100644 index 00000000..6ee2bde5 --- /dev/null +++ b/rosidl_generator_java/cmake/ament_java_install_package.cmake @@ -0,0 +1,82 @@ +# Copyright 2014 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. + +# +# Install a Python package (and its recursive subpackages). +# +# :param package_name: the Python package name +# :type package_name: string +# :param PACKAGE_DIR: the path to the Python package directory (default: +# folder relative to the CMAKE_CURRENT_LIST_DIR) +# :type PACKAGE_DIR: string +# +macro(ament_java_install_package) + _ament_cmake_java_register_environment_hook() +endmacro() + +# register environment hook for JAVAPATH once +macro(_ament_cmake_java_register_environment_hook) + if(NOT DEFINED _AMENT_CMAKE_JAVA_ENVIRONMENT_HOOK_REGISTERED) + set(_AMENT_CMAKE_JAVA_ENVIRONMENT_HOOK_REGISTERED TRUE) + + _ament_cmake_java_get_java_install_dir() + set(JAVAPATH "$AMENT_CURRENT_PREFIX/share/${PROJECT_NAME}/java/*") + + find_package(ament_cmake_core QUIET REQUIRED) + + message("INFO _ament_cmake_java_register_environment_hook") + + # backup variable + set(_JAVA_INSTALL_DIR "${JAVA_INSTALL_DIR}") + # use native separators in environment hook to match what pure Python packages do + file(TO_NATIVE_PATH "${JAVA_INSTALL_DIR}" JAVA_INSTALL_DIR) + ament_environment_hooks( + "${ament_cmake_package_templates_ENVIRONMENT_HOOK_JAVAPATH}") + # restore variable + set(JAVA_INSTALL_DIR "${_JAVA_INSTALL_DIR}") + endif() +endmacro() + +macro(_ament_cmake_java_get_java_install_dir) + if(NOT DEFINED JAVA_INSTALL_DIR) + + message("INFO _ament_cmake_java_get_java_install_dir") + # avoid storing backslash in cached variable since CMake will interpret it + # as escape character + set(_java_code + "from distutils.sysconfig import get_python_lib" + "import os" + "print(os.path.relpath(get_python_lib(prefix='${CMAKE_INSTALL_PREFIX}'), start='${CMAKE_INSTALL_PREFIX}').replace(os.sep, '/'))" + ) + execute_process( + COMMAND + "${PYTHON_EXECUTABLE}" + "-c" + "${_java_code}" + OUTPUT_VARIABLE _output + RESULT_VARIABLE _result + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + if(NOT _result EQUAL 0) + message(FATAL_ERROR + "execute_process(${PYTHON_EXECUTABLE} -c '${_java_code}') returned " + "error code ${_result}") + endif() + + set(JAVA_INSTALL_DIR + "${_output}" + CACHE INTERNAL + "The directory for Python library installation. This needs to be in JAVAPATH when 'setup.py install' is called.") + endif() +endmacro() diff --git a/rosidl_generator_java/cmake/package_templates/templates_2_cmake.py b/rosidl_generator_java/cmake/package_templates/templates_2_cmake.py new file mode 100644 index 00000000..9ef69569 --- /dev/null +++ b/rosidl_generator_java/cmake/package_templates/templates_2_cmake.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 + +# Copyright 2014-2015 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. + +from __future__ import print_function +import argparse +import os +import sys + +from ament_package.templates import get_environment_hook_template_path +from ament_package.templates import get_package_level_template_names +from ament_package.templates import get_package_level_template_path +from ament_package.templates import get_prefix_level_template_names +from ament_package.templates import get_prefix_level_template_path + +import pkg_resources + +IS_WINDOWS = os.name == 'nt' + + +def main(argv=sys.argv[1:]): + """ + Extract the information about templates provided by ament_package. + + Call the API provided by ament_package and + print CMake code defining several variables containing information about + the available templates. + """ + parser = argparse.ArgumentParser( + description='Extract information about templates provided by ' + 'ament_package and print CMake code defining several ' + 'variables', + ) + parser.add_argument( + 'outfile', + nargs='?', + help='The filename where the output should be written to', + ) + args = parser.parse_args(argv) + + lines = generate_cmake_code() + if args.outfile: + basepath = os.path.dirname(args.outfile) + if not os.path.exists(basepath): + os.makedirs(basepath) + with open(args.outfile, 'w') as f: + for line in lines: + f.write('%s\n' % line) + else: + for line in lines: + print(line) + + +def get_environment_hook_template_path(name): + return pkg_resources.resource_filename('ament_build_type_gradle', 'template/environment_hook/' + name) + + +def generate_cmake_code(): + """ + Return a list of CMake set() commands containing the template information. + :returns: list of str + """ + variables = [] + + ext = '.bat.in' if IS_WINDOWS else '.sh.in' + variables.append(( + 'ENVIRONMENT_HOOK_JAVAPATH', + '"%s"' % get_environment_hook_template_path('javapath' + ext))) + + lines = [] + for (k, v) in variables: + if isinstance(v, list): + lines.append('set(ament_cmake_package_templates_%s "")' % k) + for vv in v: + lines.append('list(APPEND ament_cmake_package_templates_%s %s)' + % (k, vv)) + else: + lines.append('set(ament_cmake_package_templates_%s %s)' % (k, v)) + # Ensure backslashes are replaced with forward slashes because CMake cannot + # parse files with backslashes in it. + return [l.replace('\\', '/') for l in lines] + +if __name__ == '__main__': + main() 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 4036eb74..313fd1d4 100644 --- a/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake +++ b/rosidl_generator_java/cmake/rosidl_generator_java_generate_interfaces.cmake @@ -95,13 +95,13 @@ endforeach() set(target_dependencies "${rosidl_generator_java_BIN}" - ${rosidl_generator_java_GENERATOR_FILES} + "${rosidl_generator_java_GENERATOR_FILES}" "${rosidl_generator_java_TEMPLATE_DIR}/msg_support.entry_point.cpp.template" "${rosidl_generator_java_TEMPLATE_DIR}/srv_support.entry_point.cpp.template" "${rosidl_generator_java_TEMPLATE_DIR}/msg.java.template" "${rosidl_generator_java_TEMPLATE_DIR}/srv.java.template" - ${rosidl_generate_interfaces_IDL_FILES} - ${_dependency_files}) + "${rosidl_generate_interfaces_IDL_FILES}" + "${_dependency_files}") foreach(dep ${target_dependencies}) if(NOT EXISTS "${dep}") message(FATAL_ERROR "Target dependency '${dep}' does not exist") @@ -299,6 +299,10 @@ if(NOT rosidl_generate_interfaces_SKIP_INSTALL) get_filename_component(_msg_package_dir "${_msg_file}" DIRECTORY) get_filename_component(_msg_package_dir "${_msg_package_dir}" DIRECTORY) + ament_java_install_package(${PROJECT_NAME}) +# install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java") +# ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}.jar") + install_jar("${PROJECT_NAME}_messages_jar" "share/${PROJECT_NAME}/java") ament_export_jars("share/${PROJECT_NAME}/java/${PROJECT_NAME}_messages.jar") endif() diff --git a/rosidl_generator_java/cmake/rosidl_generator_java_get_typesupports.cmake b/rosidl_generator_java/cmake/rosidl_generator_java_get_typesupports.cmake index 7e4f890d..07db451e 100644 --- a/rosidl_generator_java/cmake/rosidl_generator_java_get_typesupports.cmake +++ b/rosidl_generator_java/cmake/rosidl_generator_java_get_typesupports.cmake @@ -1,4 +1,5 @@ # Copyright 2016 Open Source Robotics Foundation, Inc. +# Copyright 2016 Esteve Fernandez # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/rosidl_generator_java/resource/launcher.template b/rosidl_generator_java/resource/launcher.template new file mode 100644 index 00000000..ca8de13f --- /dev/null +++ b/rosidl_generator_java/resource/launcher.template @@ -0,0 +1,5 @@ +#!/bin/sh + +CLASS_MAIN="@(type_name)" + +java -cp $JAVAPATH -D$LD_LIBRARY_PATH $CLASS_MAIN "$@" diff --git a/rosidl_generator_java/resource/msg.java.template b/rosidl_generator_java/resource/msg.java.template index fddabbaa..6fb400db 100644 --- a/rosidl_generator_java/resource/msg.java.template +++ b/rosidl_generator_java/resource/msg.java.template @@ -1,97 +1,143 @@ +@{ +def upperfirst(s): + return s[0].capitalize() + s[1:] +}@ +@ +// generated from rosidl_generator_java/resource/msg.java.template +// generated code does not contain a copyright notice + +@########################################################################## +@# EmPy template for generating files Java +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - module_name (string) +@# - jni_package_name (string) +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@########################################################################## +@ package @(package_name).@(subfolder); +// IMPORTS import org.ros2.rcljava.common.RCLJavaProxy; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; +import org.ros2.rcljava.internal.message.Message; @[for field in spec.fields]@ @[ if not field.type.is_primitive_type()]@ +@ import @(field.type.pkg_name).msg.@(field.type.type); +@ @[ end if]@ @[end for]@ -public final class @(type_name) { +/** + * Java Message for ROS2(idl) of @(type_name). + */ +public final class @(type_name) implements Message { - private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); - - static { - java.lang.String typesupportIdentifier = RCLJavaProxy.getTypesupportIdentifier(); - if (typesupportIdentifier == null) { - logger.debug("Typesupport identifier can't be found"); - } else { - try { - System.loadLibrary("@(spec.base_type.pkg_name)_@(type_name)_s__" + typesupportIdentifier); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } + /** AutoLoader JNI library. */ + static { + RCLJavaProxy.loadLibrary("@(spec.base_type.pkg_name)_@(type_name)_s__" + RCLJavaProxy.getTypesupportIdentifier()); } - } - public static native long getFromJavaConverter(); - public static native long getToJavaConverter(); - public static native long getTypeSupport(); + // NATIVES + public static native long getFromJavaConverter(); + public static native long getToJavaConverter(); + public static native long getTypeSupport(); + // CONSTANTS @[for constant in spec.constants]@ +@ public static final @(get_builtin_java_type(constant.type)) @(constant.name) = @(constant_value_to_java(constant.type, constant.value)); +@ @[end for]@ + // FIELDS @[for field in spec.fields]@ - +@ @[ if field.type.is_array]@ +@ + /** List of @(field.name). */ + private java.util.List<@(get_java_type(field.type, use_primitives=False))> @(field.name) = new java.util.ArrayList<@(get_java_type(field.type, use_primitives=False))>(); @[ if field.default_value is not None]@ - private java.util.List<@(get_java_type(field.type, use_primitives=False))> @(field.name) = java.util.Arrays.asList(new @(get_java_type(field.type, use_primitives=False))[] @(value_to_java(field.type, field.default_value))); + //private java.util.List<@(get_java_type(field.type, use_primitives=False))> @(field.name) = java.util.Arrays.asList(new @(get_java_type(field.type, use_primitives=False))[] @(value_to_java(field.type, field.default_value))); @[ else]@ - private java.util.List<@(get_java_type(field.type, use_primitives=False))> @(field.name); + //private java.util.List<@(get_java_type(field.type, use_primitives=False))> @(field.name); @[ end if]@ - public final void set@(convert_lower_case_underscore_to_camel_case(field.name))(final java.util.Collection<@(get_java_type(field.type, use_primitives=False))> @(field.name)) { + /** + * @(upperfirst(field.name)) setter. + * @@param @(field.name) + */ + public final void set@(upperfirst(convert_lower_case_underscore_to_camel_case(field.name)))(final java.util.Collection<@(get_java_type(field.type, use_primitives=False))> @(field.name)) { @[ if field.type.array_size]@ @[ if field.type.is_upper_bound]@ - if(@(field.name).size() > @(field.type.array_size)) { - throw new IllegalArgumentException("List too big, maximum size allowed: @(field.type.array_size)"); + if(@(field.name).size() > @(field.type.array_size)) { + throw new IllegalArgumentException("List too big, maximum size allowed: @(field.type.array_size)"); + } @[ else]@ - if(@(field.name).size() != @(field.type.array_size)) { - throw new IllegalArgumentException("Invalid size for fixed array, must be exactly: @(field.type.array_size)"); + if(@(field.name).size() != @(field.type.array_size)) { + throw new IllegalArgumentException("Invalid size for fixed array, must be exactly: @(field.type.array_size)"); + } @[ end if]@ - } @[ end if]@ - if (@(field.name) != null) { - this.@(field.name) = new java.util.ArrayList<@(get_java_type(field.type, use_primitives=False))>(@(field.name)); + + if (@(field.name) != null) { + this.@(field.name) = new java.util.ArrayList<@(get_java_type(field.type, use_primitives=False))>(@(field.name)); + } } - } - public final java.util.List<@(get_java_type(field.type, use_primitives=False))> get@(convert_lower_case_underscore_to_camel_case(field.name))() { - if (this.@(field.name) == null) { - return null; + /** + * @(upperfirst(field.name)) getter. + * @@return @(get_java_type(field.type, use_primitives=False)) + */ + public final java.util.List<@(get_java_type(field.type, use_primitives=False))> get@(upperfirst(convert_lower_case_underscore_to_camel_case(field.name)))() { + return this.@(field.name); } - return new java.util.ArrayList<@(get_java_type(field.type, use_primitives=False))>(this.@(field.name)); - } +@ @[ else]@ -@[ if field.default_value is not None]@ - private @(get_java_type(field.type)) @(field.name) = @(value_to_java(field.type, field.default_value)); -@[ else]@ -@[ if field.type.type == 'string']@ - private @(get_java_type(field.type)) @(field.name) = ""; -@[ else]@ - private @(get_java_type(field.type)) @(field.name); +@ + /** @(field.name) field. */ +@ +@[ if (not field.type.is_primitive_type()) or (field.type.type == "string") ]@ +@ + private @(get_java_type(field.type)) @(field.name) = new @(get_java_type(field.type))(); +@[ if field.default_value is not None]@ + // private @(get_java_type(field.type)) @(field.name) = @(value_to_java(field.type, field.default_value)); @[ end if]@ +@ +@[ else]@ +@ + private @(get_java_type(field.type)) @(field.name); +@ @[ end if]@ +@ - public void set@(convert_lower_case_underscore_to_camel_case(field.name))(final @(get_java_type(field.type)) @(field.name)) { + /** + * @(upperfirst(field.name)) setter. + * @@param @(field.name) + */ + public final void set@(upperfirst(convert_lower_case_underscore_to_camel_case(field.name)))(final @(get_java_type(field.type)) @(field.name)) { @[ if field.type.string_upper_bound]@ - if(@(field.name).length() > @(field.type.string_upper_bound)) { - throw new IllegalArgumentException("String too long, maximum size allowed: @(field.type.string_upper_bound)"); - } + if(@(field.name).length() > @(field.type.string_upper_bound)) { + throw new IllegalArgumentException("String too long, maximum size allowed: @(field.type.string_upper_bound)"); + } @[ end if]@ + this.@(field.name) = @(field.name); + } - this.@(field.name) = @(field.name); - } - - public @(get_java_type(field.type)) get@(convert_lower_case_underscore_to_camel_case(field.name))() { - return this.@(field.name); - } + /** + * @(upperfirst(field.name)) getter. + * @@return @(get_java_type(field.type)) + */ + public final @(get_java_type(field.type)) get@(upperfirst(convert_lower_case_underscore_to_camel_case(field.name)))() { + return this.@(field.name); + } +@ @[ end if]@ +@ + @[end for]@ } diff --git a/rosidl_generator_java/resource/msg_support.entry_point.cpp.template b/rosidl_generator_java/resource/msg_support.entry_point.cpp.template index 881ca10c..20e045e3 100644 --- a/rosidl_generator_java/resource/msg_support.entry_point.cpp.template +++ b/rosidl_generator_java/resource/msg_support.entry_point.cpp.template @@ -1,31 +1,115 @@ +@{ +def internalMsg(type): + return '{}__{}'.format(type.pkg_name, convert_camel_case_to_lower_case_underscore(type.type)); + +# Define use type for JNI +has_array = 0; +has_bool = 0; +has_byte = 0; +has_char = 0; +has_short = 0; +has_int = 0; +has_long = 0; +has_float = 0; +has_double = 0; +has_string = 0; +use_types = []; + +for field in spec.fields: + # Check primitive type + if field.type.is_primitive_type(): + if field.type.type == 'bool' : has_bool = 1; + if field.type.type in ['byte', 'int8', 'uint8'] : has_byte = 1; + if field.type.type == 'char': has_char = 1; + if field.type.type in ['int16', 'uint16'] : has_short = 1; + if field.type.type in ['int32', 'uint32'] : has_int = 1; + if field.type.type in ['int64', 'uint64'] : has_long = 1; + if field.type.type == 'float32' : has_float = 1; + if field.type.type == 'float64' : has_double = 1; + if field.type.type == 'string' : has_string = 1; + else: + # Make disctinc list of message + use_types.append(field.type); + # Check if array + if field.type.is_array: has_array = 1; + +# Remove duplicate Type +seen = set() +unique = [] +for item in use_types: + if item.type not in seen: + unique.append(item) + seen.add(item.type) + +use_types = unique; + +msg_typename = '%s__%s__%s' % (spec.base_type.pkg_name, subfolder, spec.base_type.type); + +}@ +// generated from rosidl_generator_java/resource/msg_support_entry_point.cpp.template +// generated code does not contain a copyright notice + +@###################################################################################################################### +@# EmPy template for generating files for Java +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - module_name (string) +@# - jni_package_name (string) +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@###################################################################################################################### +@ +@ +@###################################################################################################################### +@# Include +@###################################################################################################################### +// INCLUDES #include #include #include #include #include +#include +#include + #include <@(spec.base_type.pkg_name)/@(subfolder)/@(module_name).h> #include +@[if has_string]@ +@ #include #include +@[end if]@ +@ +@[if has_array]@ +@ #include #include -#include -#include +@[end if]@ +@ +@#### Include internal function +@[for type in use_types]@ +@ +#include <@(type.pkg_name)/msg/@(convert_camel_case_to_lower_case_underscore(type.type)).h> +#include <@(type.pkg_name)/msg/@(convert_camel_case_to_lower_case_underscore(type.type))__functions.h> -@[for field in spec.fields]@ -@[ if not field.type.is_primitive_type()]@ -#include <@(field.type.pkg_name)/msg/@(convert_camel_case_to_lower_case_underscore(field.type.type)).h> -#include <@(field.type.pkg_name)/msg/@(convert_camel_case_to_lower_case_underscore(field.type.type))__functions.h> -@[ end if]@ @[end for]@ - +@#### End Include internal function +@ +@###################################################################################################################### +@# Jni Header +@###################################################################################################################### +@ +// JNI HEADER #ifdef __cplusplus extern "C" { #endif + /* * Class: @(jni_package_name)_@(subfolder)_@(type_name) * Method: getFromJavaConverter @@ -54,387 +138,725 @@ JNIEXPORT jlong JNICALL Java_@(jni_package_name)_@(subfolder)_@(jni_type_name)_g } #endif -@{ -msg_typename = '%s__%s__%s' % (spec.base_type.pkg_name, subfolder, spec.base_type.type) -}@ - +// INTERNAL FUNCTION static_assert(sizeof(jlong) >= sizeof(intptr_t), "jlong must be able to store pointers"); static JavaVM* g_vm = nullptr; +@###################################################################################################################### +@# Cache declared +@###################################################################################################################### +@ +// JNI CACHE MESSAGE +static jclass jmessage_class; +static jmethodID jmessage_constructor; + +// JNI CACHE FIELDS +@[for field in spec.fields]@ +static jfieldID jmessage_field_@(field.name)_fid; +@[end for]@ + +// JNI CACHE JAVA TYPES +@[if has_array]@ +static jclass global_jlist_class; +static jmethodID global_jlist_init_mid; +static jmethodID global_jlist_add_mid; +static jmethodID global_jlist_get_mid; +static jmethodID global_jlist_size_mid; + +@[end if]@ +@ +@[if has_bool]@ +static jclass global_jbool_class; +static jmethodID global_jbool_value_mid; +static jmethodID global_jbool_init_mid; + +@[end if]@ +@ +@[if has_byte]@ +static jclass global_jbyte_class; +static jmethodID global_jbyte_value_mid; +static jmethodID global_jbyte_init_mid; + +@[end if]@ +@ +@[if has_char]@ +static jclass global_jchar_class; +static jmethodID global_jchar_value_mid; +static jmethodID global_jchar_init_mid; + +@[end if]@ +@ +@[if has_short]@ +static jclass global_jshort_class; +static jmethodID global_jshort_value_mid; +static jmethodID global_jshort_init_mid; + +@[end if]@ +@ +@[if has_int]@ +static jclass global_jint_class; +static jmethodID global_jint_value_mid; +static jmethodID global_jint_init_mid; + +@[end if]@ +@ +@[if has_long]@ +static jclass global_jlong_class; +static jmethodID global_jlong_value_mid; +static jmethodID global_jlong_init_mid; + +@[end if]@ +@ +@[if has_float]@ +static jclass global_jfloat_class; +static jmethodID global_jfloat_value_mid; +static jmethodID global_jfloat_init_mid; + +@[end if]@ +@ +@[if has_double]@ +static jclass global_jdouble_class; +static jmethodID global_jdouble_value_mid; +static jmethodID global_jdouble_init_mid; + +@[end if]@ +@ +@[if has_string]@ +//static jclass global_jstring_class; +//static jmethodID global_jstring_value_mid; +//static jmethodID global_jstring_init_mid; + +@[end if]@ +@ +@[for type in use_types]@ +static jclass global_@(internalMsg(type))_class; +static jmethodID global_@(internalMsg(type))_to_java_converter_mid; +static jmethodID global_@(internalMsg(type))_from_java_converter_mid; + +@[end for]@ + +@###################################################################################################################### +@# Convert from Java function +@###################################################################################################################### +/** + * Convert Java message to Native message. + */ @(msg_typename) * @(spec.base_type.pkg_name)_@(type_name)__convert_from_java(jobject _jmessage_obj, @(msg_typename) * ros_message) { + // Initialize environment. JNIEnv *env = nullptr; // TODO(esteve): check return status assert(g_vm != nullptr); g_vm->GetEnv((void**)&env, JNI_VERSION_1_6); assert(env != nullptr); - if (ros_message == nullptr) - { + // Initialize ROS Native message. + if (ros_message == nullptr) { ros_message = @(msg_typename)__create(); + assert(ros_message != nullptr); } - auto _jmessage_class = env->GetObjectClass(_jmessage_obj); +@ +@## Loop on fields @[for field in spec.fields]@ -@[ if field.type.is_array] - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "Ljava/util/List;"); - jclass _jlist_@(field.name)_class = env->FindClass("java/util/List"); - jobject _jlist_@(field.name)_object = env->GetObjectField(_jmessage_obj, _jfield_@(field.name)_fid); +@ +@#### Field is one/multiple +@[ if field.type.is_array]@ +@#### Field is array (multiple) +@ + // Convert @(field.name) collection. (@(field.type)) + jobject _jlist_@(field.name)_object = env->GetObjectField(_jmessage_obj, jmessage_field_@(field.name)_fid); if (_jlist_@(field.name)_object != nullptr) { - jmethodID _jlist_@(field.name)_get_mid = env->GetMethodID(_jlist_@(field.name)_class, "get", "(I)Ljava/lang/Object;"); -@[ if field.type.array_size is None or field.type.is_upper_bound]@ - jmethodID _jlist_@(field.name)_size_mid = env->GetMethodID(_jlist_@(field.name)_class, "size", "()I"); - jint _jlist_@(field.name)_size = env->CallIntMethod(_jlist_@(field.name)_object, _jlist_@(field.name)_size_mid); -@[ if field.type.type == 'string']@ +@ +@###### Field size +@[ if field.type.array_size is None or field.type.is_upper_bound]@ +@###### Field is dynamic size +@ + jint _jlist_@(field.name)_size = env->CallIntMethod(_jlist_@(field.name)_object, global_jlist_size_mid); +@ +@######## Field size from Type +@[ if field.type.type == 'string']@ +@######## Field is String array +@ if (!rosidl_generator_c__String__Array__init(&(ros_message->@(field.name)), _jlist_@(field.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create String__Array ros_message"); } -@[ else]@ -@[ if field.type.is_primitive_type()]@ +@ +@[ elif field.type.is_primitive_type()]@ +@######## Field is Primitive array +@ if (!rosidl_generator_c__@(field.type.type)__Array__init(&(ros_message->@(field.name)), _jlist_@(field.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create @(field.type.type)__Array ros_message"); } -@[ else]@ +@ +@[ else]@ +@######## Field is Object array +@ if (!@(field.type.pkg_name)__msg__@(field.type.type)__Array__init(&(ros_message->@(field.name)), _jlist_@(field.name)_size)) { rcljava_throw_exception(env, "java/lang/IllegalStateException", "unable to create @(field.type.type)__Array ros_message"); } +@ +@[ end if]@ +@######## End Field size from Type -@[ end if]@ -@[ end if]@ auto _dest_@(field.name) = ros_message->@(field.name).data; -@[ else]@ +@ +@[ else]@ +@###### Field is static size +@ jint _jlist_@(field.name)_size = @(field.type.array_size); auto _dest_@(field.name) = ros_message->@(field.name); -@[ end if]@ - +@ +@[ end if]@ +@###### End Field size +@ for(jint i=0; i < _jlist_@(field.name)_size; ++i) { - auto element = env->CallObjectMethod(_jlist_@(field.name)_object, _jlist_@(field.name)_get_mid, i); -@[ if field.type.is_primitive_type()]@ -@[ if field.type.type == 'bool']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Boolean"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "booleanValue", "()Z"); - jboolean _jfield_@(field.name)_value = env->CallBooleanMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Byte"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "byteValue", "()B"); - jbyte _jfield_@(field.name)_value = env->CallByteMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type == 'char']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Character"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "charValue", "()C"); - jchar _jfield_@(field.name)_value = env->CallCharMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type in ['int16', 'uint16']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Short"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "shortValue", "()S"); - jshort _jfield_@(field.name)_value = env->CallShortMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type in ['int32', 'uint32']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Integer"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "intValue", "()I"); - jint _jfield_@(field.name)_value = env->CallIntMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type in ['int64', 'uint64']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Long"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "longValue", "()J"); - jlong _jfield_@(field.name)_value = env->CallLongMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type == 'float32']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Float"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "floatValue", "()F"); - jfloat _jfield_@(field.name)_value = env->CallFloatMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type == 'float64']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Double"); - jmethodID _jfield_@(field.name)_mid = env->GetMethodID( - _jfield_@(field.name)_class, "doubleValue", "()D"); - jdouble _jfield_@(field.name)_value = env->CallDoubleMethod(element, _jfield_@(field.name)_mid); - _dest_@(field.name)[i] = _jfield_@(field.name)_value; -@[ elif field.type.type == 'string']@ - jstring _jfield_@(field.name)_value = static_cast(element); - if (_jfield_@(field.name)_value != nullptr) { - const char * _str@(field.name) = env->GetStringUTFChars(_jfield_@(field.name)_value, 0); - rosidl_generator_c__String__assign( - &_dest_@(field.name)[i], _str@(field.name)); - env->ReleaseStringUTFChars(_jfield_@(field.name)_value, _str@(field.name)); + jobject element = env->CallObjectMethod(_jlist_@(field.name)_object, global_jlist_get_mid, i); +@ +@###### Load field array +@[ if field.type.is_primitive_type()]@ +@###### Load field Primitive array +@[ if field.type.type == 'bool']@ + _dest_@(field.name)[i] = env->CallBooleanMethod(element, global_jbool_value_mid); +@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ + _dest_@(field.name)[i] = env->CallByteMethod(element, global_jbyte_value_mid); +@[ elif field.type.type == 'char']@ + _dest_@(field.name)[i] = env->CallCharMethod(element, global_jchar_value_mid); +@[ elif field.type.type in ['int16', 'uint16']]@ + _dest_@(field.name)[i] = env->CallShortMethod(element, global_jshort_value_mid); +@[ elif field.type.type in ['int32', 'uint32']]@ + _dest_@(field.name)[i] = env->CallIntMethod(element, global_jint_value_mid); +@[ elif field.type.type in ['int64', 'uint64']]@ + _dest_@(field.name)[i] = env->CallLongMethod(element, global_jlong_value_mid); +@[ elif field.type.type == 'float32']@ + _dest_@(field.name)[i] = env->CallFloatMethod(element, global_jfloat_value_mid); +@[ elif field.type.type == 'float64']@ + _dest_@(field.name)[i] = env->CallDoubleMethod(element, global_jdouble_value_mid); +@[ elif field.type.type == 'string']@ +@ + jstring _jvalue_@(field.name) = static_cast(element); + if (_jvalue_@(field.name) != nullptr) { + const char * _str_@(field.name) = env->GetStringUTFChars(_jvalue_@(field.name), 0); + rosidl_generator_c__String__assign(&_dest_@(field.name)[i], _str_@(field.name)); + env->ReleaseStringUTFChars(_jvalue_@(field.name), _str_@(field.name)); } -@[ end if]@ -@[ else]@ - - jclass _jfield_@(field.name)_class = env->FindClass("@(field.type.pkg_name)/msg/@(get_java_type(field.type))"); - assert(_jfield_@(field.name)_class != nullptr); - - jmethodID _jfield_@(field.name)_from_java_converter_mid = env->GetStaticMethodID( - _jfield_@(field.name)_class, "getFromJavaConverter", "()J"); - jlong _jfield_@(field.name)_from_java_converter_ptr = env->CallStaticLongMethod( - _jfield_@(field.name)_class, _jfield_@(field.name)_from_java_converter_mid); - +@[ end if]@ +@ +@[ else]@ +@###### Load field object array +@ + jlong _jfield_@(field.name)_from_java_converter_ptr = env->CallStaticLongMethod(global_@(internalMsg(field.type))_class, global_@(internalMsg(field.type))_from_java_converter_mid); using _convert_from_java_signature_@(field.name) = @(field.type.pkg_name)__msg__@(get_java_type(field.type)) * (*)(jobject, @(field.type.pkg_name)__msg__@(get_java_type(field.type)) *); - - _convert_from_java_signature_@(field.name) convert_from_java_@(field.name) = - reinterpret_cast<_convert_from_java_signature_@(field.name)>(_jfield_@(field.name)_from_java_converter_ptr); - + _convert_from_java_signature_@(field.name) convert_from_java_@(field.name) = reinterpret_cast<_convert_from_java_signature_@(field.name)>(_jfield_@(field.name)_from_java_converter_ptr); _dest_@(field.name)[i] = *convert_from_java_@(field.name)(element, nullptr); -@[ end if]@ +@ +@[ end if]@ +@###### Load field array +@ + env->DeleteLocalRef(element); } } -@[ else]@ -@[ if field.type.type == 'string']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "Ljava/lang/String;"); - jstring _jvalue@(field.name) = static_cast(env->GetObjectField(_jmessage_obj, _jfield_@(field.name)_fid)); - if (_jvalue@(field.name) != nullptr) { - const char * _str@(field.name) = env->GetStringUTFChars(_jvalue@(field.name), 0); - rosidl_generator_c__String__assign( - &ros_message->@(field.name), _str@(field.name)); - env->ReleaseStringUTFChars(_jvalue@(field.name), _str@(field.name)); +@ +@[ else]@ +@#### Field is not array (one) +@ + // Convert @(field.name) field. (@(field.type)) +@ +@###### Load field +@[ if field.type.type == 'string']@ + jstring _jvalue_@(field.name) = static_cast(env->GetObjectField(_jmessage_obj, jmessage_field_@(field.name)_fid)); + if (_jvalue_@(field.name) != nullptr) { + const char * _str_@(field.name) = env->GetStringUTFChars(_jvalue_@(field.name), 0); + rosidl_generator_c__String__assign(&ros_message->@(field.name), _str_@(field.name)); + env->ReleaseStringUTFChars(_jvalue_@(field.name), _str_@(field.name)); } -@[ elif field.type.type == 'bool']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "Z"); - ros_message->@(field.name) = env->GetBooleanField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "B"); - ros_message->@(field.name) = env->GetByteField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ elif field.type.type == 'char']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "C"); - ros_message->@(field.name) = env->GetCharField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ elif field.type.type in ['int16', 'uint16']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "S"); - ros_message->@(field.name) = env->GetShortField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ elif field.type.type in ['int32', 'uint32']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "I"); - ros_message->@(field.name) = env->GetIntField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ elif field.type.type in ['int64', 'uint64']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "J"); - ros_message->@(field.name) = env->GetLongField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ elif field.type.type == 'float32']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "F"); - ros_message->@(field.name) = env->GetFloatField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ elif field.type.type == 'float64']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "D"); - ros_message->@(field.name) = env->GetDoubleField(_jmessage_obj, _jfield_@(field.name)_fid); -@[ else]@ - auto _jfield_@(field.name)_fid = env->GetFieldID( - _jmessage_class, "@(field.name)", "L@(field.type.pkg_name)/msg/@(get_java_type(field.type));"); - assert(_jfield_@(field.name)_fid != nullptr); - - jobject _jfield_@(field.name)_obj = env->GetObjectField(_jmessage_obj, _jfield_@(field.name)_fid); - +@[ elif field.type.type == 'bool']@ + ros_message->@(field.name) = env->GetBooleanField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ + ros_message->@(field.name) = env->GetByteField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ elif field.type.type == 'char']@ + ros_message->@(field.name) = env->GetCharField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ elif field.type.type in ['int16', 'uint16']]@ + ros_message->@(field.name) = env->GetShortField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ elif field.type.type in ['int32', 'uint32']]@ + ros_message->@(field.name) = env->GetIntField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ elif field.type.type in ['int64', 'uint64']]@ + ros_message->@(field.name) = env->GetLongField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ elif field.type.type == 'float32']@ + ros_message->@(field.name) = env->GetFloatField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ elif field.type.type == 'float64']@ + ros_message->@(field.name) = env->GetDoubleField(_jmessage_obj, jmessage_field_@(field.name)_fid); +@[ else]@ +@ + jobject _jfield_@(field.name)_obj = env->GetObjectField(_jmessage_obj, jmessage_field_@(field.name)_fid); if (_jfield_@(field.name)_obj != nullptr) { - auto _jfield_@(field.name)_class = env->FindClass( - "@(field.type.pkg_name)/msg/@(get_java_type(field.type))"); - assert(_jfield_@(field.name)_class != nullptr); - - jmethodID _jfield_@(field.name)_from_java_converter_mid = env->GetStaticMethodID( - _jfield_@(field.name)_class, "getFromJavaConverter", "()J"); - jlong _jfield_@(field.name)_from_java_converter_ptr = env->CallStaticLongMethod( - _jfield_@(field.name)_class, _jfield_@(field.name)_from_java_converter_mid); + jlong _jfield_@(field.name)_from_java_converter_ptr = env->CallStaticLongMethod(global_@(internalMsg(field.type))_class, global_@(internalMsg(field.type))_from_java_converter_mid); using _convert_from_java_signature_@(field.name) = @(field.type.pkg_name)__msg__@(get_java_type(field.type)) * (*)(jobject, @(field.type.pkg_name)__msg__@(get_java_type(field.type)) *); - - _convert_from_java_signature_@(field.name) convert_from_java_@(field.name) = - reinterpret_cast<_convert_from_java_signature_@(field.name)>(_jfield_@(field.name)_from_java_converter_ptr); - + _convert_from_java_signature_@(field.name) convert_from_java_@(field.name) = reinterpret_cast<_convert_from_java_signature_@(field.name)>(_jfield_@(field.name)_from_java_converter_ptr); ros_message->@(field.name) = *convert_from_java_@(field.name)(_jfield_@(field.name)_obj, nullptr); } -@[ end if]@ + env->DeleteLocalRef(_jfield_@(field.name)_obj); @[ end if]@ +@###### End Load field +@ +@[ end if]@ +@#### End Field is one/multiple +@ @[end for]@ +@## End Loop on fields + + // Check and return ROS Native message. assert(ros_message != nullptr); return ros_message; } + +@###################################################################################################################### +@# Convert to Java function +@###################################################################################################################### +/** + * Convert Native message to Java message. + */ jobject @(spec.base_type.pkg_name)_@(type_name)__convert_to_java(@(msg_typename) * _ros_message, jobject _jmessage_obj) { + // Initialize environment. JNIEnv *env = nullptr; // TODO(esteve): check return status assert(g_vm != nullptr); g_vm->GetEnv((void**)&env, JNI_VERSION_1_6); assert(env != nullptr); - jclass _jmessage_class = env->FindClass("@(spec.base_type.pkg_name)/@(subfolder)/@(spec.base_type.type)"); - if (_jmessage_obj == nullptr) - { - jmethodID _jmessage_constructor = env->GetMethodID(_jmessage_class, "", "()V"); - _jmessage_obj = env->NewObject(_jmessage_class, _jmessage_constructor); + // Initialize ROS Java message. + if (_jmessage_obj == nullptr) { + _jmessage_obj = env->NewObject(jmessage_class, jmessage_constructor); + assert(_jmessage_obj != nullptr); } +@## Loop on fields @[for field in spec.fields]@ -@[ if field.type.is_array]@ -@[ if field.type.is_primitive_type()]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "Ljava/util/List;"); - jclass _jarray_list_@(field.name)_class = env->FindClass("java/util/ArrayList"); - jmethodID _jarray_list_@(field.name)_init_mid = env->GetMethodID(_jarray_list_@(field.name)_class, "", "()V"); - jobject _jarray_list_@(field.name)_obj = env->NewObject(_jarray_list_@(field.name)_class, _jarray_list_@(field.name)_init_mid); -@[ if field.type.array_size and not field.type.is_upper_bound]@ - for(size_t i = 0; i < @(field.type.array_size); ++i) { - auto _ros_@(field.name)_element = _ros_message->@(field.name)[i]; -@[ else]@ - for(size_t i = 0; i < _ros_message->@(field.name).size; ++i) { - auto _ros_@(field.name)_element = _ros_message->@(field.name).data[i]; -@[ end if]@ -@[ if field.type.type == 'string']@ + +@#### Field is one/multiple +@[ if field.type.is_array]@ +@#### Field is array (multiple) +@ + // Convert @(field.name) collection. (@(field.type)) +@ +@###### Field size +@[ if field.type.array_size is None or field.type.is_upper_bound]@ +@###### Field is dynamic size + jint _jlist_@(field.name)_size = _ros_message->@(field.name).size; +@ +@[ else]@ +@###### Field is static size +@ + jint _jlist_@(field.name)_size = @(field.type.array_size); +@ +@[ end if]@ +@###### End Field size +@ + jobject _jarray_list_@(field.name)_obj = env->NewObject(global_jlist_class, global_jlist_init_mid); + for(jint i = 0; i < _jlist_@(field.name)_size; ++i) { +@ +@[ if field.type.array_size is None or field.type.is_upper_bound]@ +@#### Field is String array #### +@[ if field.type.type == 'string']@ jobject _jlist_@(field.name)_element = nullptr; - if (_ros_@(field.name)_element.data != nullptr) { - _jlist_@(field.name)_element = env->NewStringUTF(_ros_@(field.name)_element.data); + if (_ros_message->@(field.name).data[i].data != nullptr) { + _jlist_@(field.name)_element = env->NewStringUTF(_ros_message->@(field.name).data[i].data); } -@[ else]@ -@[ if field.type.type == 'bool']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Boolean"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(Z)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, _ros_@(field.name)_element); -@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Byte"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(B)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, static_cast(_ros_@(field.name)_element)); -@[ elif field.type.type == 'char']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Character"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(C)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, _ros_@(field.name)_element); -@[ elif field.type.type in ['int16', 'uint16']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Short"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(S)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, _ros_@(field.name)_element); -@[ elif field.type.type in ['int32', 'uint32']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Integer"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(I)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, _ros_@(field.name)_element); -@[ elif field.type.type in ['int64', 'uint64']]@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Long"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(J)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, _ros_@(field.name)_element); -@[ elif field.type.type == 'float32']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Float"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(F)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, _ros_@(field.name)_element); -@[ elif field.type.type == 'float64']@ - jclass _jfield_@(field.name)_class = env->FindClass("java/lang/Double"); - jmethodID _jlist_@(field.name)_init_mid = env->GetMethodID(_jfield_@(field.name)_class, "", "(D)V"); - jobject _jlist_@(field.name)_element = env->NewObject(_jfield_@(field.name)_class, _jlist_@(field.name)_init_mid, _ros_@(field.name)_element); -@[ end if]@ -@[ end if]@ - // TODO(esteve): replace ArrayList with a jobjectArray to initialize the array beforehand - jmethodID _jlist_@(field.name)_add_mid = env->GetMethodID(_jarray_list_@(field.name)_class, "add", "(Ljava/lang/Object;)Z"); - if (_jlist_@(field.name)_element != nullptr) { - jboolean _jlist_@(field.name)_add_result = env->CallBooleanMethod(_jarray_list_@(field.name)_obj, _jlist_@(field.name)_add_mid, _jlist_@(field.name)_element); - assert(_jlist_@(field.name)_add_result); +@[ elif field.type.type == 'bool']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jbool_class, global_jbool_init_mid, _ros_message->@(field.name).data[i]); +@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jbyte_class, global_jbyte_init_mid, static_cast(_ros_message->@(field.name).data[i])); +@[ elif field.type.type == 'char']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jchar_class, global_jchar_init_mid, _ros_message->@(field.name).data[i]); +@[ elif field.type.type in ['int16', 'uint16']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jshort_class, global_jshort_init_mid, _ros_message->@(field.name).data[i]); +@[ elif field.type.type in ['int32', 'uint32']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jint_class, global_jint_init_mid, _ros_message->@(field.name).data[i]); +@[ elif field.type.type in ['int64', 'uint64']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jlong_class, global_jlong_init_mid, _ros_message->@(field.name).data[i]); +@[ elif field.type.type == 'float32']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jfloat_class, global_jfloat_init_mid, _ros_message->@(field.name).data[i]); +@[ elif field.type.type == 'float64']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jdouble_class, global_jdouble_init_mid, _ros_message->@(field.name).data[i]); +@[ else]@ +@ + jlong _jfield_@(field.name)_to_java_converter_ptr = env->CallStaticLongMethod(global_@(internalMsg(field.type))_class, global_@(internalMsg(field.type))_to_java_converter_mid); + using _convert_to_java_signature_@(field.name) = jobject (*)(@(field.type.pkg_name)__msg__@(get_java_type(field.type)) *, jobject); + _convert_to_java_signature_@(field.name) convert_to_java_@(field.name) = reinterpret_cast<_convert_to_java_signature_@(field.name)>(_jfield_@(field.name)_to_java_converter_ptr); + jobject _jlist_@(field.name)_element = convert_to_java_@(field.name)(&(_ros_message->@(field.name).data[i]), nullptr); +@[ end if]@ +@ +@[ else]@ +@###### Field is static size +@ +@#### Field is String array #### +@[ if field.type.type == 'string']@ + jobject _jlist_@(field.name)_element = nullptr; + if (_ros_message->@(field.name).data[i].data != nullptr) { + _jlist_@(field.name)_element = env->NewStringUTF(_ros_message->@(field.name)[i].data); } - } - - env->SetObjectField(_jmessage_obj, _jfield_@(field.name)_fid, _jarray_list_@(field.name)_obj); -@[ else]@ - - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "Ljava/util/List;"); - jclass _jarray_list_@(field.name)_class = env->FindClass("java/util/ArrayList"); - jmethodID _jarray_list_@(field.name)_init_mid = env->GetMethodID(_jarray_list_@(field.name)_class, "", "()V"); - jobject _jarray_list_@(field.name)_obj = env->NewObject(_jarray_list_@(field.name)_class, _jarray_list_@(field.name)_init_mid); - - jclass _jfield_@(field.name)_class = env->FindClass("@(field.type.pkg_name)/msg/@(get_java_type(field.type))"); - assert(_jfield_@(field.name)_class != nullptr); - - jmethodID _jfield_@(field.name)_to_java_converter_mid = env->GetStaticMethodID( - _jfield_@(field.name)_class, "getToJavaConverter", "()J"); - jlong _jfield_@(field.name)_to_java_converter_ptr = env->CallStaticLongMethod( - _jfield_@(field.name)_class, _jfield_@(field.name)_to_java_converter_mid); - - using _convert_to_java_signature_@(field.name) = jobject (*)(@(field.type.pkg_name)__msg__@(get_java_type(field.type)) *, jobject); - - _convert_to_java_signature_@(field.name) convert_to_java_@(field.name) = - reinterpret_cast<_convert_to_java_signature_@(field.name)>(_jfield_@(field.name)_to_java_converter_ptr); - -@[ if field.type.array_size and not field.type.is_upper_bound]@ - for(size_t i = 0; i < @(field.type.array_size); ++i) { - jobject _jlist_@(field.name)_element = convert_to_java_@(field.name)(&(_ros_message->@(field.name)[i]), nullptr); -@[ else]@ - for(size_t i = 0; i < _ros_message->@(field.name).size; ++i) { - jobject _jlist_@(field.name)_element = convert_to_java_@(field.name)(&(_ros_message->@(field.name).data[i]), nullptr); -@[ end if]@ - // TODO(esteve): replace ArrayList with a jobjectArray to initialize the array beforehand - jmethodID _jlist_@(field.name)_add_mid = env->GetMethodID(_jarray_list_@(field.name)_class, "add", "(Ljava/lang/Object;)Z"); +@[ elif field.type.type == 'bool']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jbool_class, global_jbool_init_mid, _ros_message->@(field.name)[i]); +@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jbyte_class, global_jbyte_init_mid, static_cast(_ros_message->@(field.name)[i])); +@[ elif field.type.type == 'char']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jchar_class, global_jchar_init_mid, _ros_message->@(field.name)[i]); +@[ elif field.type.type in ['int16', 'uint16']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jshort_class, global_jshort_init_mid, _ros_message->@(field.name)[i]); +@[ elif field.type.type in ['int32', 'uint32']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jint_class, global_jint_init_mid, _ros_message->@(field.name)[i]); +@[ elif field.type.type in ['int64', 'uint64']]@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jlong_class, global_jlong_init_mid, _ros_message->@(field.name)[i]); +@[ elif field.type.type == 'float32']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jfloat_class, global_jfloat_init_mid, _ros_message->@(field.name)[i]); +@[ elif field.type.type == 'float64']@ + jobject _jlist_@(field.name)_element = env->NewObject(global_jdouble_class, global_jdouble_init_mid, _ros_message->@(field.name)[i]); +@[ else]@ +@ + jlong _jfield_@(field.name)_to_java_converter_ptr = env->CallStaticLongMethod(global_@(internalMsg(field.type))_class, global_@(internalMsg(field.type))_to_java_converter_mid); + using _convert_to_java_signature_@(field.name) = jobject (*)(@(field.type.pkg_name)__msg__@(get_java_type(field.type)) *, jobject); + _convert_to_java_signature_@(field.name) convert_to_java_@(field.name) = reinterpret_cast<_convert_to_java_signature_@(field.name)>(_jfield_@(field.name)_to_java_converter_ptr); + jobject _jlist_@(field.name)_element = convert_to_java_@(field.name)(&(_ros_message->@(field.name)[i]), nullptr); +@[ end if]@ +@[ end if]@ +@ + // TODO (Theos) check why are not element... if (_jlist_@(field.name)_element != nullptr) { - jboolean _jlist_@(field.name)_add_result = env->CallBooleanMethod(_jarray_list_@(field.name)_obj, _jlist_@(field.name)_add_mid, _jlist_@(field.name)_element); + jboolean _jlist_@(field.name)_add_result = env->CallBooleanMethod(_jarray_list_@(field.name)_obj, global_jlist_add_mid, _jlist_@(field.name)_element); assert(_jlist_@(field.name)_add_result); } - } - - env->SetObjectField(_jmessage_obj, _jfield_@(field.name)_fid, _jarray_list_@(field.name)_obj); -@[ end if]@ -@[ else]@ + env->SetObjectField(_jmessage_obj, jmessage_field_@(field.name)_fid, _jarray_list_@(field.name)_obj); + env->DeleteLocalRef(_jarray_list_@(field.name)_obj); +@ +@[ else]@ +@#### Field is not array #### +@ + // Convert @(field.name) field. (@(field.type)) @[ if field.type.type == 'string']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "Ljava/lang/String;"); +@ if (_ros_message->@(field.name).data != nullptr) { - env->SetObjectField(_jmessage_obj, _jfield_@(field.name)_fid, env->NewStringUTF(_ros_message->@(field.name).data)); + env->SetObjectField(_jmessage_obj, jmessage_field_@(field.name)_fid, env->NewStringUTF(_ros_message->@(field.name).data)); } +@ @[ elif field.type.type == 'bool']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "Z"); - env->SetBooleanField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetBooleanField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ elif field.type.type in ['byte', 'int8', 'uint8']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "B"); - env->SetByteField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetByteField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ elif field.type.type == 'char']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "C"); - env->SetCharField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetCharField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ elif field.type.type in ['int16', 'uint16']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "S"); - env->SetShortField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetShortField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ elif field.type.type in ['int32', 'uint32']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "I"); - env->SetIntField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetIntField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ elif field.type.type in ['int64', 'uint64']]@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "J"); - env->SetLongField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetLongField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ elif field.type.type == 'float32']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "F"); - env->SetFloatField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetFloatField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ elif field.type.type == 'float64']@ - auto _jfield_@(field.name)_fid = env->GetFieldID(_jmessage_class, "@(field.name)", "D"); - env->SetDoubleField(_jmessage_obj, _jfield_@(field.name)_fid, _ros_message->@(field.name)); + env->SetDoubleField(_jmessage_obj, jmessage_field_@(field.name)_fid, _ros_message->@(field.name)); @[ else]@ - auto _jfield_@(field.name)_fid = env->GetFieldID( - _jmessage_class, "@(field.name)", "L@(field.type.pkg_name)/msg/@(get_java_type(field.type));"); - assert(_jfield_@(field.name)_fid != nullptr); - - auto _jfield_@(field.name)_class = env->FindClass("@(field.type.pkg_name)/msg/@(get_java_type(field.type))"); - assert(_jfield_@(field.name)_class != nullptr); - - jmethodID _jfield_@(field.name)_to_java_converter_mid = env->GetStaticMethodID( - _jfield_@(field.name)_class, "getToJavaConverter", "()J"); - jlong _jfield_@(field.name)_to_java_converter_ptr = env->CallStaticLongMethod( - _jfield_@(field.name)_class, _jfield_@(field.name)_to_java_converter_mid); - +@ + jlong _jfield_@(field.name)_to_java_converter_ptr = env->CallStaticLongMethod(global_@(internalMsg(field.type))_class, global_@(internalMsg(field.type))_to_java_converter_mid); using _convert_to_java_signature_@(field.name) = jobject (*)(@(field.type.pkg_name)__msg__@(get_java_type(field.type)) *, jobject); - - _convert_to_java_signature_@(field.name) convert_to_java_@(field.name) = - reinterpret_cast<_convert_to_java_signature_@(field.name)>(_jfield_@(field.name)_to_java_converter_ptr); - + _convert_to_java_signature_@(field.name) convert_to_java_@(field.name) = reinterpret_cast<_convert_to_java_signature_@(field.name)>(_jfield_@(field.name)_to_java_converter_ptr); jobject _jfield_@(field.name)_obj = convert_to_java_@(field.name)(&(_ros_message->@(field.name)), nullptr); - env->SetObjectField(_jmessage_obj, _jfield_@(field.name)_fid, _jfield_@(field.name)_obj); + env->SetObjectField(_jmessage_obj, jmessage_field_@(field.name)_fid, _jfield_@(field.name)_obj); +@ @[ end if]@ - -@[ end if]@ +@ +@[ end if]@ +@#### End Field is one/multiple +@ @[end for]@ +@## End Loop on fields + + // Check and return ROS Native message. assert(_jmessage_obj != nullptr); return _jmessage_obj; } + +@###################################################################################################################### +@# JNI Loader +@###################################################################################################################### +// JNI LOADER JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void*) { // Can only call this once if(g_vm == nullptr) { g_vm = vm; + + JNIEnv* env; + if (g_vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return JNI_ERR; + } else { + //// Add Message Java + jclass _jmessage_class = env->FindClass("@(spec.base_type.pkg_name)/@(subfolder)/@(spec.base_type.type)"); + jmessage_class = reinterpret_cast (env->NewGlobalRef(_jmessage_class)); + env->DeleteLocalRef(_jmessage_class); + assert(jmessage_class != nullptr); + jmessage_constructor = env->GetMethodID(jmessage_class, "", "()V"); + assert(jmessage_constructor != nullptr); + + //// Add Fields Java +@[for field in spec.fields]@ +@ + // @(field.name) field. (@(field.type)) +@[ if field.type.is_array]@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "Ljava/util/List;"); + assert(jmessage_field_@(field.name)_fid != nullptr); +@[ elif field.type.type == 'bool']@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "Z"); +@[ elif field.type.type in ['byte', 'int8', 'uint8']]@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "B"); +@[ elif field.type.type == 'char']@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "C"); +@[ elif field.type.type in ['int16', 'uint16']]@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "S"); +@[ elif field.type.type in ['int32', 'uint32']]@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "I"); +@[ elif field.type.type in ['int64', 'uint64']]@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "J"); +@[ elif field.type.type == 'float32']@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "F"); +@[ elif field.type.type == 'float64']@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "D"); +@[ elif field.type.type == 'string']@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "Ljava/lang/String;"); + assert(jmessage_field_@(field.name)_fid != nullptr); +@[ else]@ + jmessage_field_@(field.name)_fid = env->GetFieldID(jmessage_class, "@(field.name)", "L@(field.type.pkg_name)/msg/@(get_java_type(field.type));"); + assert(jmessage_field_@(field.name)_fid != nullptr); +@[ end if]@ + +@[end for]@ +@ + //// Add Array Java Class +@[if has_array]@ + jclass _list_class = env->FindClass("java/util/ArrayList"); + global_jlist_class = (jclass) env->NewGlobalRef(_list_class); + env->DeleteLocalRef(_list_class); + assert(global_jlist_class != nullptr); + global_jlist_init_mid = env->GetMethodID(global_jlist_class, "", "()V"); + global_jlist_add_mid = env->GetMethodID(global_jlist_class, "add", "(Ljava/lang/Object;)Z"); + global_jlist_get_mid = env->GetMethodID(global_jlist_class, "get", "(I)Ljava/lang/Object;"); + global_jlist_size_mid = env->GetMethodID(global_jlist_class, "size", "()I"); + +@[end if]@ +@ + //// Cache Primitive type class +@[if has_bool]@ + // Bool Type + jclass _jbool_class = env->FindClass("java/lang/Boolean"); + global_jbool_class = (jclass) env->NewGlobalRef(_jbool_class); + env->DeleteLocalRef(_jbool_class); + assert(global_jbool_class != nullptr); + global_jbool_init_mid = env->GetMethodID(global_jbool_class, "", "(Z)V"); + global_jbool_value_mid = env->GetMethodID(global_jbool_class, "booleanValue", "()Z"); + +@[end if]@ +@ +@[if has_byte]@ + // Byte Type + jclass _jbyte_class = env->FindClass("java/lang/Byte"); + global_jbyte_class = (jclass) env->NewGlobalRef(_jbyte_class); + env->DeleteLocalRef(_jbyte_class); + assert(global_jbyte_class != nullptr); + global_jbyte_init_mid = env->GetMethodID(global_jbyte_class, "", "(B)V"); + global_jbyte_value_mid = env->GetMethodID(global_jbyte_class, "byteValue", "()B"); + +@[end if]@ +@ +@[if has_char]@ + // Char Type + jclass _jchar_class = env->FindClass("java/lang/Char"); + global_jchar_class = (jclass) env->NewGlobalRef(_jchar_class); + env->DeleteLocalRef(_jchar_class); + assert(global_jchar_class != nullptr); + global_jchar_init_mid = env->GetMethodID(global_jchar_class, "", "(C)V"); + global_jchar_value_mid = env->GetMethodID(global_jchar_class, "charValue", "()C"); + +@[end if]@ +@ +@[if has_short]@ + // Short Type + jclass _jshort_class = env->FindClass("java/lang/Short"); + global_jshort_class = (jclass) env->NewGlobalRef(_jshort_class); + env->DeleteLocalRef(_jshort_class); + assert(global_jshort_class != nullptr); + global_jshort_init_mid = env->GetMethodID(global_jshort_class, "", "(S)V"); + global_jshort_value_mid = env->GetMethodID(global_jshort_class, "shortValue", "()S"); + +@[end if]@ +@ +@[if has_int]@ + // Integer Type + jclass _jint_class = env->FindClass("java/lang/Integer"); + global_jint_class = (jclass) env->NewGlobalRef(_jint_class); + env->DeleteLocalRef(_jint_class); + assert(global_jint_class != nullptr); + global_jint_init_mid = env->GetMethodID(global_jint_class, "", "(I)V"); + global_jint_value_mid = env->GetMethodID(global_jint_class, "intValue", "()I"); + +@[end if]@ +@ +@[if has_long]@ + // Long Type + jclass _jlong_class = env->FindClass("java/lang/Long"); + global_jlong_class = (jclass) env->NewGlobalRef(_jlong_class); + env->DeleteLocalRef(_jlong_class); + assert(global_jlong_class != nullptr); + global_jlong_init_mid = env->GetMethodID(global_jlong_class, "", "(J)V"); + global_jlong_value_mid = env->GetMethodID(global_jlong_class, "longValue", "()J"); + +@[end if]@ +@ +@[if has_float]@ + // Float Type + jclass _jfloat_class = env->FindClass("java/lang/Float"); + global_jfloat_class = (jclass) env->NewGlobalRef(_jfloat_class); + env->DeleteLocalRef(_jfloat_class); + assert(global_jfloat_class != nullptr); + global_jfloat_init_mid = env->GetMethodID(global_jfloat_class, "", "(F)V"); + global_jfloat_value_mid = env->GetMethodID(global_jfloat_class, "floatValue", "()F"); + +@[end if]@ +@ +@[if has_double]@ + // Double Type + jclass _jdouble_class = env->FindClass("java/lang/Double"); + global_jdouble_class = (jclass) env->NewGlobalRef(_jdouble_class); + env->DeleteLocalRef(_jdouble_class); + assert(global_jdouble_class != nullptr); + global_jdouble_init_mid = env->GetMethodID(global_jdouble_class, "", "(D)V"); + global_jdouble_value_mid = env->GetMethodID(global_jdouble_class, "doubleValue", "()D"); + +@[end if]@ +@ +@[if has_string]@ +// // String Type +// jclass _jstring_class = env->FindClass("java/lang/String"); +// global_jstring_class = (jclass) env->NewGlobalRef(_jstring_class); +// env->DeleteLocalRef(_jstring_class); +// assert(global_jstring_class != nullptr); +// global_jstring_init_mid = env->GetMethodID(global_jstring_class, "", "(L)V"); +// global_jstring_value_mid = env->GetMethodID(global_jstring_class, "stringValue", "()Ljava/lang/String;"); + +@[end if]@ +@ + //// Add Sub-Message Java Class +@[for type in use_types]@ +@ + // @(type). + jclass _@(internalMsg(type))_class = env->FindClass("@(type.pkg_name)/msg/@(get_java_type(type))"); + assert(_@(internalMsg(type))_class != nullptr); + global_@(internalMsg(type))_class = (jclass) env->NewGlobalRef(_@(internalMsg(type))_class); + env->DeleteLocalRef(_@(internalMsg(type))_class); + assert(global_@(internalMsg(type))_class != nullptr); + global_@(internalMsg(type))_to_java_converter_mid = env->GetStaticMethodID(global_@(internalMsg(type))_class, "getToJavaConverter", "()J"); + global_@(internalMsg(type))_from_java_converter_mid = env->GetStaticMethodID(global_@(internalMsg(type))_class, "getFromJavaConverter", "()J"); + +@[end for]@ +@ + } } + return JNI_VERSION_1_6; } +JNIEXPORT void JNICALL JNI_OnUnload(JavaVM *, void*) { + JNIEnv* env; + if (g_vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return; + } else { + // delete global references so the GC can collect them + if (jmessage_class != nullptr) { + env->DeleteGlobalRef(jmessage_class); + } + +@[if has_array]@ + if (global_jlist_class != nullptr) { + env->DeleteGlobalRef(global_jlist_class); + } + +@[end if]@ +@ +@[if has_bool]@ + if (global_jbool_class != nullptr) { + env->DeleteGlobalRef(global_jbool_class); + } + +@[end if]@ +@ +@[if has_byte]@ + if (global_jbyte_class != nullptr) { + env->DeleteGlobalRef(global_jbyte_class); + } + +@[end if]@ +@ +@[if has_char]@ + if (global_jchar_class != nullptr) { + env->DeleteGlobalRef(global_jchar_class); + } + +@[end if]@ +@ +@[if has_short]@ + if (global_jshort_class != nullptr) { + env->DeleteGlobalRef(global_jshort_class); + } + +@[end if]@ +@ +@[if has_int]@ + if (global_jint_class != nullptr) { + env->DeleteGlobalRef(global_jint_class); + } + +@[end if]@ +@ +@[if has_long]@ + if (global_jlong_class != nullptr) { + env->DeleteGlobalRef(global_jlong_class); + } + +@[end if]@ +@ +@[if has_float]@ + if (global_jfloat_class != nullptr) { + env->DeleteGlobalRef(global_jfloat_class); + } + +@[end if]@ +@ +@[if has_double]@ + if (global_jdouble_class != nullptr) { + env->DeleteGlobalRef(global_jdouble_class); + } + +@[end if]@ +@ +@[for type in use_types]@ + if (global_@(internalMsg(type))_class != nullptr) { + env->DeleteGlobalRef(global_@(internalMsg(type))_class); + } + +@[end for]@ + } + return; +} + + +@###################################################################################################################### +@# JNI body +@###################################################################################################################### +// JNI BODY JNIEXPORT jlong JNICALL Java_@(jni_package_name)_@(subfolder)_@(jni_type_name)_getFromJavaConverter (JNIEnv *, jclass) { diff --git a/rosidl_generator_java/resource/srv.java.template b/rosidl_generator_java/resource/srv.java.template index c2b8663d..e973b71f 100644 --- a/rosidl_generator_java/resource/srv.java.template +++ b/rosidl_generator_java/resource/srv.java.template @@ -1,31 +1,53 @@ +// generated from rosidl_generator_java/resource/srv.java.template +// generated code does not contain a copyright notice + +@########################################################################## +@# EmPy template for generating files Java +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - module_name (string) +@# - jni_package_name (string) +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@########################################################################## +@ package @(package_name).@(subfolder); +// IMPORTS import org.ros2.rcljava.common.RCLJavaProxy; +import org.ros2.rcljava.internal.message.Message; +import org.ros2.rcljava.internal.service.Service; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; +import @(package_name).@(subfolder).@(type_name)_Request; +import @(package_name).@(subfolder).@(type_name)_Response; -public class @(type_name) { +/** + * Java Service for ROS2(idl) of @(type_name). + */ +public class @(type_name) implements Service { - private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); - - static { - java.lang.String typesupportIdentifier = RCLJavaProxy.getTypesupportIdentifier(); - if (typesupportIdentifier == null) { - logger.debug("Typesupport identifier can't be found"); - } else { - try { - System.loadLibrary("@(spec.pkg_name)_@(type_name)_s__" + typesupportIdentifier); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } + /** AutoLoader JNI library. */ + static { + RCLJavaProxy.loadLibrary("@(spec.pkg_name)_@(type_name)_s__" + RCLJavaProxy.getTypesupportIdentifier()); } - } - public static native long getServiceTypeSupport(); + // NATIVES + public static native long getServiceTypeSupport(); + + // INTERNALS COMPONENTS + public static final Class<@(type_name)_Request> RequestType = @(type_name)_Request.class; - public static final Class<@(type_name)_Request> RequestType = @(type_name)_Request.class; + public static final Class<@(type_name)_Response> ResponseType = @(type_name)_Response.class; - public static final Class<@(type_name)_Response> ResponseType = @(type_name)_Response.class; + @@Override + public Class getRequestType() { + return @(type_name)_Request.class; + } + + @@Override + public Class getResponseType() { + return @(type_name)_Response.class; + } } diff --git a/rosidl_generator_java/resource/srv_support.entry_point.cpp.template b/rosidl_generator_java/resource/srv_support.entry_point.cpp.template index 96ad99e6..cca4d898 100644 --- a/rosidl_generator_java/resource/srv_support.entry_point.cpp.template +++ b/rosidl_generator_java/resource/srv_support.entry_point.cpp.template @@ -1,8 +1,25 @@ +// generated from rosidl_generator_java/resource/srv_support_entry_point.cpp.template +// generated code does not contain a copyright notice + +@########################################################################## +@# EmPy template for generating files for Java +@# Context: +@# - spec (rosidl_parser.MessageSpecification) +@# Parsed specification of the .msg file +@# - module_name (string) +@# - jni_package_name (string) +@# - subfolder (string) +@# The subfolder / subnamespace of the message +@# Either 'msg' or 'srv' +@########################################################################## +@ +// INCLUDES #include #include #include <@(spec.pkg_name)/@(subfolder)/@(module_name).h> +// JNI HEADER #ifdef __cplusplus extern "C" { #endif @@ -19,10 +36,11 @@ JNIEXPORT jlong JNICALL Java_@(jni_package_name)_@(subfolder)_@(jni_type_name)_g } #endif +// INTERNAL FUNCTION + JNIEXPORT jlong JNICALL Java_@(jni_package_name)_@(subfolder)_@(jni_type_name)_getServiceTypeSupport (JNIEnv *, jclass) { - const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( - @(spec.pkg_name), @(subfolder), @(spec.srv_name)); + const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(@(spec.pkg_name), @(subfolder), @(spec.srv_name)); return reinterpret_cast(ts); } diff --git a/rosidl_generator_java/rosidl_generator_java/__init__.py b/rosidl_generator_java/rosidl_generator_java/__init__.py index 04b8d6cf..bc747fef 100644 --- a/rosidl_generator_java/rosidl_generator_java/__init__.py +++ b/rosidl_generator_java/rosidl_generator_java/__init__.py @@ -90,10 +90,12 @@ def generate_java(generator_arguments_file, typesupport_impl, typesupport_impls) 'convert_lower_case_underscore_to_camel_case': convert_lower_case_underscore_to_camel_case, 'get_builtin_java_type': get_builtin_java_type, - 'module_name': module_name, 'package_name': package_name, + 'module_name': module_name, + 'package_name': package_name, 'jni_package_name': jni_package_name, + 'spec': spec, + 'subfolder': subfolder, 'jni_type_name': jni_type_name, - 'spec': spec, 'subfolder': subfolder, 'typesupport_impl': type_support_impl_by_filename.get(generated_filename, ''), 'typesupport_impls': typesupport_impls, 'type_name': type_name, @@ -220,3 +222,10 @@ def get_java_type(type_, use_primitives=True): return type_.type return get_builtin_java_type(type_.type, use_primitives=use_primitives) + + +def convert_lower_case_underscore_to_camel_case(value): + components = value.split('_') + # We capitalize the first letter of each component except the first one + # with the 'title' method and join them together. + return components[0] + "".join(x.title() for x in components[1:]) diff --git a/rosidl_ros2_android.diff b/rosidl_ros2_android.diff new file mode 100644 index 00000000..9fd6be10 --- /dev/null +++ b/rosidl_ros2_android.diff @@ -0,0 +1,43 @@ +diff --git a/rosidl_default_generators/CMakeLists.txt b/rosidl_default_generators/CMakeLists.txt +index d6fda02..e616910 100644 +--- a/rosidl_default_generators/CMakeLists.txt ++++ b/rosidl_default_generators/CMakeLists.txt +@@ -7,8 +7,7 @@ find_package(ament_cmake REQUIRED) + ament_export_dependencies(rosidl_cmake) + + ament_export_dependencies(rosidl_generator_c) +-ament_export_dependencies(rosidl_generator_cpp) +-ament_export_dependencies(rosidl_generator_py) ++ament_export_dependencies(rosidl_generator_java) + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) +diff --git a/rosidl_default_generators/package.xml b/rosidl_default_generators/package.xml +index f7d303f..a54e967 100644 +--- a/rosidl_default_generators/package.xml ++++ b/rosidl_default_generators/package.xml +@@ -12,18 +12,19 @@ + rosidl_cmake + + rosidl_generator_c +- rosidl_generator_cpp +- rosidl_generator_py ++ rosidl_generator_java ++ + + rosidl_typesupport_cpp + + rosidl_typesupport_introspection_c +- rosidl_typesupport_introspection_cpp ++ + + rosidl_typesupport_connext_c +- rosidl_typesupport_connext_cpp ++ + rosidl_typesupport_opensplice_c +- rosidl_typesupport_opensplice_cpp ++ + + ament_lint_auto + ament_lint_common diff --git a/rosidl_ros2_java.diff b/rosidl_ros2_java.diff new file mode 100644 index 00000000..fdb45dc0 --- /dev/null +++ b/rosidl_ros2_java.diff @@ -0,0 +1,24 @@ +diff --git a/rosidl_default_generators/CMakeLists.txt b/rosidl_default_generators/CMakeLists.txt +index d6fda02..886eab6 100644 +--- a/rosidl_default_generators/CMakeLists.txt ++++ b/rosidl_default_generators/CMakeLists.txt +@@ -8,6 +8,7 @@ ament_export_dependencies(rosidl_cmake) + + ament_export_dependencies(rosidl_generator_c) + ament_export_dependencies(rosidl_generator_cpp) ++ament_export_dependencies(rosidl_generator_java) + ament_export_dependencies(rosidl_generator_py) + + if(BUILD_TESTING) +diff --git a/rosidl_default_generators/package.xml b/rosidl_default_generators/package.xml +index 31b481d..b33263f 100644 +--- a/rosidl_default_generators/package.xml ++++ b/rosidl_default_generators/package.xml +@@ -13,6 +13,7 @@ + + rosidl_generator_c + rosidl_generator_cpp ++ rosidl_generator_java + rosidl_generator_py + + rosidl_typesupport_introspection_c