From 4a5d229fecdb74d87a18322dc16ae5a20cdfef4a Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 23 Oct 2020 18:51:34 +0000 Subject: [PATCH 01/32] Add interfaces for action goal, result, and feedback Implementing these interfaces in the code generation template makes it easier to pass around these types in a generic way. Note, the 'final' modifier had to be removed from generated message types in order to extend goal, result, and feedback types in action definitions. Signed-off-by: Jacob Perron --- .../ros2/rcljava/interfaces/ActionDefinition.java | 6 +++++- rosidl_generator_java/resource/action.java.em | 12 ++++++------ rosidl_generator_java/resource/msg.java.em | 2 +- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java index 9681463b..87694fa1 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java @@ -15,4 +15,8 @@ package org.ros2.rcljava.interfaces; -public interface ActionDefinition {} +public interface ActionDefinition { + interface ActionGoal {} + interface ActionResult {} + interface ActionFeedback {} +} diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 5e7ebbe9..19fe9a57 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -76,6 +76,12 @@ import @(action_import); public class @(type_name) implements ActionDefinition { + public class Goal extends @(type_name)_Goal implements ActionGoal<@(type_name)> {} + + public class Result extends @(type_name)_Result implements ActionResult<@(type_name)> {} + + public class Feedback extends @(type_name)_Feedback implements ActionFeedback<@(type_name)> {} + private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); static { @@ -88,10 +94,4 @@ public class @(type_name) implements ActionDefinition { } public static native long getActionTypeSupport(); - - public static final Class<@(type_name)_Goal> GoalType = @(type_name)_Goal.class; - - public static final Class<@(type_name)_Result> ResultType = @(type_name)_Result.class; - - public static final Class<@(type_name)_Feedback> FeedbackType = @(type_name)_Feedback.class; } diff --git a/rosidl_generator_java/resource/msg.java.em b/rosidl_generator_java/resource/msg.java.em index c8912488..9dcb2a42 100644 --- a/rosidl_generator_java/resource/msg.java.em +++ b/rosidl_generator_java/resource/msg.java.em @@ -38,7 +38,7 @@ import @('.'.join(member.type.namespaced_name())); @[ end if]@ @[end for]@ -public final class @(type_name) implements MessageDefinition { +public class @(type_name) implements MessageDefinition { private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); From 768b6d470200dad2aa5946a465d941b56dc091d0 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 29 Oct 2020 01:36:58 +0000 Subject: [PATCH 02/32] Add new definitions for action goal response and request Signed-off-by: Jacob Perron --- rcljava_common/CMakeLists.txt | 2 ++ .../rcljava/interfaces/ActionDefinition.java | 5 +++ .../interfaces/GoalRequestDefinition.java | 21 +++++++++++++ .../interfaces/GoalResponseDefinition.java | 21 +++++++++++++ rosidl_generator_java/resource/action.java.em | 31 +++++++++++++++++++ 5 files changed, 80 insertions(+) create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java create mode 100644 rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java diff --git a/rcljava_common/CMakeLists.txt b/rcljava_common/CMakeLists.txt index 3bc04fde..46715393 100644 --- a/rcljava_common/CMakeLists.txt +++ b/rcljava_common/CMakeLists.txt @@ -38,6 +38,8 @@ set(${PROJECT_NAME}_java_sources "src/main/java/org/ros2/rcljava/exceptions/RCLReturn.java" "src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/Disposable.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java" + "src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/MessageDefinition.java" "src/main/java/org/ros2/rcljava/interfaces/ServiceDefinition.java" ) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java index 87694fa1..63134367 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java @@ -19,4 +19,9 @@ public interface ActionDefinition { interface ActionGoal {} interface ActionResult {} interface ActionFeedback {} + + Class getSendGoalRequestType(); + Class getSendGoalResponseType(); + Class getGetResultRequestType(); + Class getGetResultResponseType(); } diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java new file mode 100644 index 00000000..f7c96111 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -0,0 +1,21 @@ +/* Copyright 2020 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.interfaces; + +public interface GoalRequestDefinition extends MessageDefinition { + ActionDefinition.ActionGoal getAbstractGoal(); + // TODO: Add getGoalId(); +} diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java new file mode 100644 index 00000000..22a23979 --- /dev/null +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java @@ -0,0 +1,21 @@ +/* Copyright 2020 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.interfaces; + +public interface GoalResponseDefinition extends MessageDefinition { + void accept(boolean accepted); + // TODO: Add setStamp(); +} diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 19fe9a57..bad47669 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -65,6 +65,9 @@ expand_template( action_imports = [ 'org.ros2.rcljava.common.JNIUtils', 'org.ros2.rcljava.interfaces.ActionDefinition', + 'org.ros2.rcljava.interfaces.GoalRequestDefinition', + 'org.ros2.rcljava.interfaces.GoalResponseDefinition', + 'org.ros2.rcljava.interfaces.MessageDefinition', 'org.slf4j.Logger', 'org.slf4j.LoggerFactory', ] @@ -82,6 +85,34 @@ public class @(type_name) implements ActionDefinition { public class Feedback extends @(type_name)_Feedback implements ActionFeedback<@(type_name)> {} + public class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition { + public ActionDefinition.ActionGoal getAbstractGoal() { + return (Goal)super.getGoal(); + } + } + + public class SendGoalResponse extends @(type_name)_SendGoal_Response implements GoalResponseDefinition { + public void accept(boolean accepted) { + super.setAccepted(accepted); + } + } + + public Class getSendGoalRequestType() { + return SendGoalRequest.class; + } + + public Class getSendGoalResponseType() { + return SendGoalResponse.class; + } + + public Class getGetResultRequestType() { + return @(type_name)_GetResult_Request.class; + } + + public Class getGetResultResponseType() { + return @(type_name)_GetResult_Response.class; + } + private static final Logger logger = LoggerFactory.getLogger(@(type_name).class); static { From bdecfa7aef77d5013c6b42f91bd354b7d2e50396 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 6 Nov 2020 23:23:52 +0000 Subject: [PATCH 03/32] Add getter for UUID to SendGoalRequest Also make inner classes static. Signed-off-by: Jacob Perron --- .../rcljava/interfaces/GoalRequestDefinition.java | 6 ++++-- rosidl_generator_java/resource/action.java.em | 15 ++++++++------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java index f7c96111..d70f099e 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -15,7 +15,9 @@ package org.ros2.rcljava.interfaces; +import java.util.List; + public interface GoalRequestDefinition extends MessageDefinition { - ActionDefinition.ActionGoal getAbstractGoal(); - // TODO: Add getGoalId(); + MessageDefinition getGoal(); + List getGoalUuid(); } diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index bad47669..7bb91571 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -63,6 +63,7 @@ expand_template( template_basepath=template_basepath) action_imports = [ + 'java.util.List', 'org.ros2.rcljava.common.JNIUtils', 'org.ros2.rcljava.interfaces.ActionDefinition', 'org.ros2.rcljava.interfaces.GoalRequestDefinition', @@ -79,19 +80,19 @@ import @(action_import); public class @(type_name) implements ActionDefinition { - public class Goal extends @(type_name)_Goal implements ActionGoal<@(type_name)> {} + public static class Goal extends @(type_name)_Goal implements ActionGoal<@(type_name)> {} - public class Result extends @(type_name)_Result implements ActionResult<@(type_name)> {} + public static class Result extends @(type_name)_Result implements ActionResult<@(type_name)> {} - public class Feedback extends @(type_name)_Feedback implements ActionFeedback<@(type_name)> {} + public static class Feedback extends @(type_name)_Feedback implements ActionFeedback<@(type_name)> {} - public class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition { - public ActionDefinition.ActionGoal getAbstractGoal() { - return (Goal)super.getGoal(); + public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition { + public List getGoalUuid() { + return super.getGoalId().getUuid(); } } - public class SendGoalResponse extends @(type_name)_SendGoal_Response implements GoalResponseDefinition { + public static class SendGoalResponse extends @(type_name)_SendGoal_Response implements GoalResponseDefinition { public void accept(boolean accepted) { super.setAccepted(accepted); } From 1b65d7fd91a95cd7e112c33d1f4ea8f2312a6bb6 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 13 Nov 2020 09:08:12 -0800 Subject: [PATCH 04/32] Add getStamp method to GoalResponseDefinition Signed-off-by: Jacob Perron --- .../ros2/rcljava/interfaces/GoalResponseDefinition.java | 2 +- rosidl_generator_java/resource/action.java.em | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java index 22a23979..c802c58e 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java @@ -17,5 +17,5 @@ public interface GoalResponseDefinition extends MessageDefinition { void accept(boolean accepted); - // TODO: Add setStamp(); + void setStamp(int sec, int nanosec); } diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 7bb91571..cadb15e3 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -96,6 +96,13 @@ public class @(type_name) implements ActionDefinition { public void accept(boolean accepted) { super.setAccepted(accepted); } + + public void setStamp(int sec, int nanosec) { + builtin_interfaces.msg.Time msg = new builtin_interfaces.msg.Time(); + msg.setSec(sec); + msg.setNanosec(nanosec); + super.setStamp(msg); + } } public Class getSendGoalRequestType() { From b725144feda52eadff2e1d8b46d995bab12d0298 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 13 Nov 2020 09:19:07 -0800 Subject: [PATCH 05/32] Partially revert "Add interfaces for action goal, result, and feedback" Partially revert commit dd046147d38c1aa844376149f62c4f3aaec04d96. I don't think we need to aliases for the message types, but I'll add them back if they turn out to be useful. --- .../ros2/rcljava/interfaces/ActionDefinition.java | 4 ---- rosidl_generator_java/resource/action.java.em | 12 ++++++------ 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java index 63134367..e39d65d8 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/ActionDefinition.java @@ -16,10 +16,6 @@ package org.ros2.rcljava.interfaces; public interface ActionDefinition { - interface ActionGoal {} - interface ActionResult {} - interface ActionFeedback {} - Class getSendGoalRequestType(); Class getSendGoalResponseType(); Class getGetResultRequestType(); diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index cadb15e3..7b158e0c 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -80,12 +80,6 @@ import @(action_import); public class @(type_name) implements ActionDefinition { - public static class Goal extends @(type_name)_Goal implements ActionGoal<@(type_name)> {} - - public static class Result extends @(type_name)_Result implements ActionResult<@(type_name)> {} - - public static class Feedback extends @(type_name)_Feedback implements ActionFeedback<@(type_name)> {} - public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition { public List getGoalUuid() { return super.getGoalId().getUuid(); @@ -133,4 +127,10 @@ public class @(type_name) implements ActionDefinition { } public static native long getActionTypeSupport(); + + public static final Class<@(type_name)_Goal> GoalType = @(type_name)_Goal.class; + + public static final Class<@(type_name)_Result> ResultType = @(type_name)_Result.class; + + public static final Class<@(type_name)_Feedback> FeedbackType = @(type_name)_Feedback.class; } From f7d7f557acc63f05804a33daaa212e13090bb208 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Jan 2021 12:15:14 -0800 Subject: [PATCH 06/32] Parameterize goal request and response interfaces on action type Signed-off-by: Jacob Perron --- .../org/ros2/rcljava/interfaces/GoalRequestDefinition.java | 2 +- .../org/ros2/rcljava/interfaces/GoalResponseDefinition.java | 2 +- rosidl_generator_java/resource/action.java.em | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java index d70f099e..cf6dc177 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -17,7 +17,7 @@ import java.util.List; -public interface GoalRequestDefinition extends MessageDefinition { +public interface GoalRequestDefinition extends MessageDefinition { MessageDefinition getGoal(); List getGoalUuid(); } diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java index c802c58e..c645cf8f 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalResponseDefinition.java @@ -15,7 +15,7 @@ package org.ros2.rcljava.interfaces; -public interface GoalResponseDefinition extends MessageDefinition { +public interface GoalResponseDefinition extends MessageDefinition { void accept(boolean accepted); void setStamp(int sec, int nanosec); } diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 7b158e0c..12fb29f7 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -80,13 +80,13 @@ import @(action_import); public class @(type_name) implements ActionDefinition { - public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition { + public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition<@(type_name)> { public List getGoalUuid() { return super.getGoalId().getUuid(); } } - public static class SendGoalResponse extends @(type_name)_SendGoal_Response implements GoalResponseDefinition { + public static class SendGoalResponse extends @(type_name)_SendGoal_Response implements GoalResponseDefinition<@(type_name)> { public void accept(boolean accepted) { super.setAccepted(accepted); } From 7a6a47f645c5afedbb39b760c0ec685dfcb0b5ef Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Jan 2021 11:55:43 -0800 Subject: [PATCH 07/32] Fix getGoalUuid implementation It should return a List, since it is hashable. Signed-off-by: Jacob Perron --- rosidl_generator_java/resource/action.java.em | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rosidl_generator_java/resource/action.java.em b/rosidl_generator_java/resource/action.java.em index 12fb29f7..cb6c1dd3 100644 --- a/rosidl_generator_java/resource/action.java.em +++ b/rosidl_generator_java/resource/action.java.em @@ -82,7 +82,8 @@ public class @(type_name) implements ActionDefinition { public static class SendGoalRequest extends @(type_name)_SendGoal_Request implements GoalRequestDefinition<@(type_name)> { public List getGoalUuid() { - return super.getGoalId().getUuid(); + // Return List since it's hash is based on the values (not the object pointer) + return super.getGoalId().getUuidAsList(); } } From 63200b3af16a1afd3d01f69e868b8524e4db5851 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Jan 2021 11:23:35 -0800 Subject: [PATCH 08/32] List -> byte[] Signed-off-by: Jacob Perron --- .../java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java index cf6dc177..af7b9a84 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -19,5 +19,5 @@ public interface GoalRequestDefinition extends MessageDefinition { MessageDefinition getGoal(); - List getGoalUuid(); + byte[] getGoalUuid(); } From cd347890b9809d4a100aa30696db0b8edac2102d Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 14 Oct 2020 22:28:07 +0000 Subject: [PATCH 09/32] Add ActionServer skeleton * Add new action module with classes/interfaces related to the ActionServer * Add methods for creating and removing ActionServers from a Node * Implement dispose method for ActionServer. Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 10 ++ ...org_ros2_rcljava_action_ActionServerImpl.h | 43 +++++++ rcljava/package.xml | 2 + ...g_ros2_rcljava_action_ActionServerImpl.cpp | 71 +++++++++++ .../org/ros2/rcljava/action/ActionServer.java | 23 ++++ .../action/ActionServerGoalHandle.java | 22 ++++ .../ros2/rcljava/action/ActionServerImpl.java | 112 ++++++++++++++++++ .../ros2/rcljava/action/CancelCallback.java | 28 +++++ .../ros2/rcljava/action/CancelResponse.java | 23 ++++ .../org/ros2/rcljava/action/GoalCallback.java | 29 +++++ .../org/ros2/rcljava/action/GoalResponse.java | 23 ++++ .../main/java/org/ros2/rcljava/node/Node.java | 28 +++++ .../java/org/ros2/rcljava/node/NodeImpl.java | 38 ++++++ 13 files changed, 452 insertions(+) create mode 100644 rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h create mode 100644 rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index b357f0d4..98601be2 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(ament_cmake_export_jars REQUIRED) find_package(ament_cmake_export_jni_libraries REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rcl REQUIRED) +find_package(rcl_action REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcljava_common REQUIRED) find_package(rmw REQUIRED) @@ -57,6 +58,7 @@ endif() set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_RCLJava.cpp" "src/main/cpp/org_ros2_rcljava_Time.cpp" + "src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp" "src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp" "src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp" "src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp" @@ -106,6 +108,7 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) ament_target_dependencies(${_target_name} "rcl" + "rcl_action" "rcljava_common" "builtin_interfaces" "rcl_interfaces" @@ -129,6 +132,13 @@ endforeach() set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/RCLJava.java" "src/main/java/org/ros2/rcljava/Time.java" + "src/main/java/org/ros2/rcljava/action/CancelCallback.java" + "src/main/java/org/ros2/rcljava/action/CancelResponse.java" + "src/main/java/org/ros2/rcljava/action/GoalCallback.java" + "src/main/java/org/ros2/rcljava/action/GoalResponse.java" + "src/main/java/org/ros2/rcljava/action/ActionServer.java" + "src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java" + "src/main/java/org/ros2/rcljava/action/ActionServerImpl.java" "src/main/java/org/ros2/rcljava/client/Client.java" "src/main/java/org/ros2/rcljava/client/ClientImpl.java" "src/main/java/org/ros2/rcljava/concurrent/Callback.java" diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h new file mode 100644 index 00000000..50bf8c05 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -0,0 +1,43 @@ +// Copyright 2020 ros2-java contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +/* Header for class org_ros2_rcljava_action_ActionServerImpl */ + +#ifndef ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H_ +#define ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeDispose + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose(JNIEnv *, jclass, jlong, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeCreateActionServer + * Signature: (JLjava/lang/Class;Ljava/lang/String;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( + JNIEnv *, jobject, jlong, jclass, jstring); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_H__ diff --git a/rcljava/package.xml b/rcljava/package.xml index 9e32719b..48413010 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -15,6 +15,7 @@ builtin_interfaces rcl_interfaces rcl + rcl_action rcpputils rmw_implementation_cmake rmw @@ -32,6 +33,7 @@ builtin_interfaces rcl_interfaces rcl + rcl_action rcpputils rmw_implementation_cmake rmw_implementation diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp new file mode 100644 index 00000000..b0cf5c16 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -0,0 +1,71 @@ +// Copyright 2020 ros2-java contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl_action/rcl_action.h" +#include "rosidl_generator_c/message_type_support_struct.h" + +#include "rcljava_common/exceptions.hpp" +#include "rcljava_common/signatures.hpp" + +#include "org_ros2_rcljava_action_ActionServerImpl.h" + +using rcljava_common::exceptions::rcljava_throw_rclexception; + +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose( + JNIEnv * env, jclass, jlong node_handle, jlong action_server_handle) +{ + if (action_server_handle == 0) { + // everything is ok, already destroyed + return; + } + + if (node_handle == 0) { + // TODO(jacobperron): throw exception + return; + } + + rcl_node_t * node = reinterpret_cast(node_handle); + + assert(node != NULL); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + + assert(action_server != NULL); + + rcl_ret_t ret = rcl_action_server_fini(action_server, node); + + if (ret != RCL_RET_OK) { + std::string msg = "Failed to destroy action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT jlong JNICALL +Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( + JNIEnv *, jobject, jlong node_handle, jclass action_type, jstring action_name) +{ + return 0; +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java new file mode 100644 index 00000000..df2615ae --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java @@ -0,0 +1,23 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +import org.ros2.rcljava.interfaces.Disposable; +// import org.ros2.rcljava.interfaces.MessageDefinition; +import org.ros2.rcljava.interfaces.ActionDefinition; + +public interface ActionServer extends Disposable { +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java new file mode 100644 index 00000000..eff0e5e6 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -0,0 +1,22 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +import org.ros2.rcljava.interfaces.ActionDefinition; + +public class ActionServerGoalHandle { + +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java new file mode 100644 index 00000000..c600d775 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -0,0 +1,112 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +import java.lang.ref.WeakReference; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.common.JNIUtils; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.interfaces.MessageDefinition; +import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.node.Node; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class ActionServerImpl implements ActionServer { + private static final Logger logger = LoggerFactory.getLogger(ActionServerImpl.class); + + static { + try { + JNIUtils.loadImplementation(ActionServerImpl.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + private final WeakReference nodeReference; + private final String actionName; + private long handle; + private final GoalCallback goalCallback; + private final CancelCallback cancelCallback; + private final Consumer> acceptedCallback; + + private native long nativeCreateActionServer( + long nodeHandle, Class cls, String actionName); + + /** + * Create an action server. + * + * @param nodeReference A reference to the node to use to create this action server. + * @param actionType The type of the action. + * @param actionName The name of the action. + * @param goalCallback Callback triggered when a new goal request is received. + * @param cancelCallback Callback triggered when a new cancel request is received. + * @param acceptedCallback Callback triggered when a new goal is accepted. + */ + public ActionServerImpl( + final WeakReference nodeReference, + final Class actionType, + final String actionName, + final GoalCallback goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) { + this.nodeReference = nodeReference; + this.actionName = actionName; + this.goalCallback = goalCallback; + this.cancelCallback = cancelCallback; + this.acceptedCallback = acceptedCallback; + + Node node = nodeReference.get(); + if (node != null) { + // TODO: throw + } + + this.handle = nativeCreateActionServer(node.getHandle(), actionType, actionName); + // TODO(jacobperron): Introduce 'Waitable' interface for entities like timers, services, etc + // node.addWaitable(this); + } + + /** + * Destroy the underlying rcl_action_server_t. + * + * @param nodeHandle A pointer to the underlying rcl_node_t handle that + * created this action server. + * @param handle A pointer to the underlying rcl_action_server_t + */ + private static native void nativeDispose(long nodeHandle, long handle); + + /** + * {@inheritDoc} + */ + public final void dispose() { + Node node = this.nodeReference.get(); + if (node != null) { + nativeDispose(node.getHandle(), this.handle); + node.removeActionServer(this); + this.handle = 0; + } + } + + /** + * {@inheritDoc} + */ + public final long getHandle() { + return handle; + } +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java new file mode 100644 index 00000000..a7203c5e --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java @@ -0,0 +1,28 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +import org.ros2.rcljava.interfaces.ActionDefinition; + +public interface CancelCallback { + /** + * Called when a new cancel request is received. + * + * @param goalHandle The goal handle. + * @return Cancel response indicating if the cancel request was accepted or not. + */ + CancelResponse handleCancel(ActionServerGoalHandle goalHandle); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java b/rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java new file mode 100644 index 00000000..8695bee8 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java @@ -0,0 +1,23 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +// TODO(jacobperron): Consider moving inside Server interface +// ie. Server.CancelResponse.ACCEPT +enum CancelResponse { + REJECT, + ACCEPT, +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java new file mode 100644 index 00000000..c6e17b4b --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -0,0 +1,29 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +import org.ros2.rcljava.interfaces.MessageDefinition; + +public interface GoalCallback { + /** + * Called when a new goal request is received. + * + * @param goal The action goal request. + * @return Goal response indicating if the goal was accepted or not. + */ + // TODO(jacobperron): Add UUID parameter + GoalResponse handleGoal(T goal); +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java new file mode 100644 index 00000000..6318f821 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java @@ -0,0 +1,23 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +// TODO(jacobperron): Consider moving inside Server interface +// ie. Server.GoalResponse.ACCEPT +enum GoalResponse { + REJECT, + ACCEPT, +} diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index b6750e39..af3f944a 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -20,6 +20,10 @@ import java.util.Map; import java.util.concurrent.TimeUnit; +import org.ros2.rcljava.action.ActionServer; +import org.ros2.rcljava.action.ActionServerGoalHandle; +import org.ros2.rcljava.action.CancelCallback; +import org.ros2.rcljava.action.GoalCallback; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.concurrent.Callback; import org.ros2.rcljava.consumers.Consumer; @@ -28,6 +32,7 @@ import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.parameters.ParameterCallback; @@ -77,6 +82,11 @@ public interface Node extends Disposable { */ Collection getTimers(); + /** + * @return All the @{link ActionServer}s that were created by this instance. + */ + Collection getActionServers(); + /** * Create a Subscription<T>. * @@ -133,6 +143,12 @@ Client createClient( Client createClient(final Class serviceType, final String serviceName) throws NoSuchFieldException, IllegalAccessException; + ActionServer createActionServer(final Class actionType, + final String actionName, + final GoalCallback goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback); + /** * Remove a Subscription created by this Node. * @@ -181,6 +197,18 @@ Client createClient(final Class serviceType, */ boolean removeClient(final Client client); + /** + * Remove an @{link ActionServer} created by this Node. + * + * Calling this method effectively invalidates the passed @{link ActionServer}. + * If the server was not created by this Node, then nothing happens. + * + * @param actionServer The object to remove from this node. + * @return true if the server was removed, false if the server was already + * removed or was never created by this Node. + */ + boolean removeActionServer(final ActionServer actionServer); + /** * Create a wall timer. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java index 687138cb..47022ae1 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -16,6 +16,11 @@ package org.ros2.rcljava.node; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.action.ActionServer; +import org.ros2.rcljava.action.ActionServerImpl; +import org.ros2.rcljava.action.ActionServerGoalHandle; +import org.ros2.rcljava.action.CancelCallback; +import org.ros2.rcljava.action.GoalCallback; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.client.ClientImpl; import org.ros2.rcljava.common.JNIUtils; @@ -27,6 +32,7 @@ import org.ros2.rcljava.graph.EndpointInfo; import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.ActionDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.node.NodeOptions; @@ -135,6 +141,11 @@ public class NodeImpl implements Node { */ private final Collection timers; + /** + * All the @{link ActionServer}s that have been created through this instance. + */ + private final Collection actionServers; + private Object parametersMutex; class ParameterAndDescriptor { @@ -169,6 +180,7 @@ public NodeImpl(final long handle, final NodeOptions nodeOptions) { this.services = new LinkedBlockingQueue(); this.clients = new LinkedBlockingQueue(); this.timers = new LinkedBlockingQueue(); + this.actionServers = new LinkedBlockingQueue(); this.parametersMutex = new Object(); this.parameters = new ConcurrentHashMap(); this.allowUndeclaredParameters = nodeOptions.getAllowUndeclaredParameters(); @@ -388,6 +400,18 @@ public Client createClient(final Class servi private static native long nativeCreateClientHandle( long handle, Class cls, String serviceName, long qosProfileHandle); + public ActionServer createActionServer(final Class actionType, + final String actionName, + final GoalCallback goalCallback, + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) { + ActionServer actionServer = new ActionServerImpl( + new WeakReference(this), actionType, actionName, + goalCallback, cancelCallback, acceptedCallback); + this.actionServers.add(actionServer); + return actionServer; + } + /** * {@inheritDoc} */ @@ -402,6 +426,13 @@ public boolean removeClient(final Client client) { return this.clients.remove(client); } + /** + * {@inheritDoc} + */ + public boolean removeActionServer(final ActionServer actionServer) { + return this.actionServers.remove(actionServer); + } + /** * {@inheritDoc} */ @@ -479,6 +510,13 @@ public final Collection getTimers() { return this.timers; } + /** + * {@inheritDoc} + */ + public final Collection getActionServers() { + return this.actionServers; + } + /** * {@inheritDoc} */ From f988e401674b723ad74dbb92b8a25f06f5ece5e4 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Sat, 17 Oct 2020 00:41:24 +0000 Subject: [PATCH 10/32] Add ActionServer creation logic and unit tests Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 13 ++++ ...org_ros2_rcljava_action_ActionServerImpl.h | 2 +- rcljava/package.xml | 1 + ...g_ros2_rcljava_action_ActionServerImpl.cpp | 42 ++++++++++++- .../ros2/rcljava/action/ActionServerImpl.java | 13 ++-- .../main/java/org/ros2/rcljava/node/Node.java | 4 +- .../java/org/ros2/rcljava/node/NodeImpl.java | 4 +- .../ros2/rcljava/action/ActionServerTest.java | 63 +++++++++++++++++++ 8 files changed, 129 insertions(+), 13 deletions(-) create mode 100644 rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index 98601be2..cebaba6d 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -228,6 +228,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(std_msgs REQUIRED) find_package(mockito_vendor REQUIRED) + find_package(test_msgs REQUIRED) ament_lint_auto_find_test_dependencies() set(${PROJECT_NAME}_message_files @@ -274,6 +275,7 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/RCLJavaTest.java" "src/test/java/org/ros2/rcljava/SpinTest.java" "src/test/java/org/ros2/rcljava/TimeTest.java" + "src/test/java/org/ros2/rcljava/action/ActionServerTest.java" "src/test/java/org/ros2/rcljava/client/ClientTest.java" "src/test/java/org/ros2/rcljava/contexts/ContextTest.java" "src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java" @@ -293,6 +295,7 @@ if(BUILD_TESTING) "org.ros2.rcljava.RCLJavaTest" "org.ros2.rcljava.SpinTest" "org.ros2.rcljava.TimeTest" + "org.ros2.rcljava.action.ActionServerTest" "org.ros2.rcljava.client.ClientTest" "org.ros2.rcljava.contexts.ContextTest" "org.ros2.rcljava.node.NodeOptionsTest" @@ -354,6 +357,15 @@ if(BUILD_TESTING) list_append_unique(_deps_library_dirs ${_dep_dir}) endforeach() + foreach(_dep_lib ${test_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${test_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rcljava) list_append_unique(_deps_library_dirs ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_java/rcljava/msg/) @@ -375,6 +387,7 @@ if(BUILD_TESTING) "${builtin_interfaces_JARS}" "${rcl_interfaces_JARS}" "${rosgraph_msgs_JARS}" + "${test_msgs_JARS}" "${mockito_vendor_JARS}" "${_${PROJECT_NAME}_jar_file}" "${_${PROJECT_NAME}_messages_jar_file}" diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h index 50bf8c05..dba7e007 100644 --- a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -35,7 +35,7 @@ JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose(JNIEnv *, jc */ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( - JNIEnv *, jobject, jlong, jclass, jstring); + JNIEnv *, jobject, jlong, jlong, jclass, jstring); #ifdef __cplusplus } diff --git a/rcljava/package.xml b/rcljava/package.xml index 48413010..67ac38d5 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -52,6 +52,7 @@ rosidl_runtime_c rosidl_generator_java std_msgs + test_msgs ament_cmake diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp index b0cf5c16..fa52d563 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -65,7 +65,45 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose( JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( - JNIEnv *, jobject, jlong node_handle, jclass action_type, jstring action_name) + JNIEnv * env, + jobject, + jlong node_handle, + jlong clock_handle, + jclass jaction_class, + jstring jaction_name) { - return 0; + jmethodID mid = env->GetStaticMethodID(jaction_class, "getActionTypeSupport", "()J"); + assert(mid != NULL); + + jlong jts = env->CallStaticLongMethod(jaction_class, mid); + assert(jts != 0); + + const char * action_name = env->GetStringUTFChars(jaction_name, 0); + + // std::string service_name(service_name_tmp); + // env->ReleaseStringUTFChars(jservice_name, service_name_tmp); + + rcl_node_t * node = reinterpret_cast(node_handle); + rcl_clock_t * clock = reinterpret_cast(clock_handle); + + rosidl_action_type_support_t * ts = reinterpret_cast(jts); + + rcl_action_server_t * action_server = static_cast( + malloc(sizeof(rcl_action_server_t))); + *action_server = rcl_action_get_zero_initialized_server(); + rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); + + rcl_ret_t ret = rcl_action_server_init( + action_server, node, clock, ts, action_name, &action_server_ops); + env->ReleaseStringUTFChars(jaction_name, action_name); + + if (ret != RCL_RET_OK) { + std::string msg = "Failed to create action server: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return 0; + } + + jlong jaction_server = reinterpret_cast(action_server); + return jaction_server; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index c600d775..6b5fca45 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -43,11 +43,11 @@ public class ActionServerImpl implements ActionServe private final String actionName; private long handle; private final GoalCallback goalCallback; - private final CancelCallback cancelCallback; - private final Consumer> acceptedCallback; + private final CancelCallback cancelCallback; + private final Consumer> acceptedCallback; private native long nativeCreateActionServer( - long nodeHandle, Class cls, String actionName); + long nodeHandle, long clockHandle, Class cls, String actionName); /** * Create an action server. @@ -64,8 +64,8 @@ public ActionServerImpl( final Class actionType, final String actionName, final GoalCallback goalCallback, - final CancelCallback cancelCallback, - final Consumer> acceptedCallback) { + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) { this.nodeReference = nodeReference; this.actionName = actionName; this.goalCallback = goalCallback; @@ -77,7 +77,8 @@ public ActionServerImpl( // TODO: throw } - this.handle = nativeCreateActionServer(node.getHandle(), actionType, actionName); + this.handle = nativeCreateActionServer( + node.getHandle(), node.getClock().getHandle(), actionType, actionName); // TODO(jacobperron): Introduce 'Waitable' interface for entities like timers, services, etc // node.addWaitable(this); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index af3f944a..1c47b75f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -146,8 +146,8 @@ Client createClient(final Class serviceType, ActionServer createActionServer(final Class actionType, final String actionName, final GoalCallback goalCallback, - final CancelCallback cancelCallback, - final Consumer> acceptedCallback); + final CancelCallback cancelCallback, + final Consumer> acceptedCallback); /** * Remove a Subscription created by this Node. diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java index 47022ae1..7d44726e 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -403,8 +403,8 @@ private static native long nativeCreateClientHandl public ActionServer createActionServer(final Class actionType, final String actionName, final GoalCallback goalCallback, - final CancelCallback cancelCallback, - final Consumer> acceptedCallback) { + final CancelCallback cancelCallback, + final Consumer> acceptedCallback) { ActionServer actionServer = new ActionServerImpl( new WeakReference(this), actionType, actionName, goalCallback, cancelCallback, acceptedCallback); diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java new file mode 100644 index 00000000..f1f14c86 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -0,0 +1,63 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotEquals; + +import org.junit.Test; + +import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; + +public class ActionServerTest { + @Test + public final void testCreateAndDispose() { + RCLJava.rclJavaInit(); + Node node = RCLJava.createNode("test_node"); + Consumer> acceptedCallback = + new Consumer>() { + public void accept(final ActionServerGoalHandle goalHandle) {} + }; + + ActionServer actionServer = + node.createActionServer( + test_msgs.action.Fibonacci.class, "test_action", + new GoalCallback() { + public GoalResponse handleGoal(test_msgs.action.Fibonacci_Goal goal) { + return GoalResponse.ACCEPT; + } + }, + new CancelCallback() { + public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { + return CancelResponse.REJECT; + } + }, + acceptedCallback); + + assertNotEquals(0, actionServer.getHandle()); + assertEquals(1, node.getActionServers().size()); + + // We expect that calling dispose should result in a zero handle + // and the reference is dropped from the Node + actionServer.dispose(); + assertEquals(0, actionServer.getHandle()); + assertEquals(0, node.getActionServers().size()); + + RCLJava.shutdown(); + } +} From 676fcae11dee0f01eae1c897c586b7769040b4bd Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 20 Oct 2020 00:05:34 +0000 Subject: [PATCH 11/32] Add action server logic for accepting and canceling goals This changeset includes the following: * an implementation of the goal handle for action servers * action server integration with the base executor * action server integration with ROS nodes * unit tests for the action server, receiving goal and cancel requests Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 37 +- ...org_ros2_rcljava_action_ActionServerImpl.h | 120 +++++ ...a_action_ActionServerImpl_GoalHandleImpl.h | 99 ++++ .../org_ros2_rcljava_executors_BaseExecutor.h | 9 + rcljava/package.xml | 4 + rcljava/src/main/cpp/convert.hpp | 89 ++++ ...g_ros2_rcljava_action_ActionServerImpl.cpp | 289 ++++++++++- ...action_ActionServerImpl_GoalHandleImpl.cpp | 149 ++++++ ...rg_ros2_rcljava_executors_BaseExecutor.cpp | 78 +-- .../org/ros2/rcljava/action/ActionServer.java | 50 +- .../action/ActionServerGoalHandle.java | 43 +- .../ros2/rcljava/action/ActionServerImpl.java | 475 +++++++++++++++++- .../org/ros2/rcljava/action/GoalStatus.java | 47 ++ .../ros2/rcljava/executors/AnyExecutable.java | 2 + .../ros2/rcljava/executors/BaseExecutor.java | 52 ++ .../main/java/org/ros2/rcljava/node/Node.java | 3 +- .../java/org/ros2/rcljava/node/NodeImpl.java | 5 +- .../ros2/rcljava/action/ActionServerTest.java | 291 ++++++++++- .../ros2/rcljava/action/MockActionClient.java | 104 ++++ .../java/org/ros2/rcljava/node/NodeTest.java | 2 +- 20 files changed, 1845 insertions(+), 103 deletions(-) create mode 100644 rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h create mode 100644 rcljava/src/main/cpp/convert.hpp create mode 100644 rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp create mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java create mode 100644 rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index cebaba6d..e2373558 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(rcljava) +find_package(action_msgs REQUIRED) find_package(ament_cmake REQUIRED) find_package(ament_cmake_export_jars REQUIRED) find_package(ament_cmake_export_jni_libraries REQUIRED) @@ -13,6 +14,7 @@ find_package(rcljava_common REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(rosgraph_msgs REQUIRED) +find_package(unique_identifier_msgs REQUIRED) include(CrossCompilingExtra) @@ -59,6 +61,7 @@ set(${PROJECT_NAME}_jni_sources "src/main/cpp/org_ros2_rcljava_RCLJava.cpp" "src/main/cpp/org_ros2_rcljava_Time.cpp" "src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp" + "src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp" "src/main/cpp/org_ros2_rcljava_client_ClientImpl.cpp" "src/main/cpp/org_ros2_rcljava_contexts_ContextImpl.cpp" "src/main/cpp/org_ros2_rcljava_detail_QosIncompatibleStatus.cpp" @@ -113,6 +116,8 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) "builtin_interfaces" "rcl_interfaces" "rosgraph_msgs" + "action_msgs" + "unique_identifier_msgs" ) target_include_directories(${_target_name} @@ -132,13 +137,14 @@ endforeach() set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/RCLJava.java" "src/main/java/org/ros2/rcljava/Time.java" + "src/main/java/org/ros2/rcljava/action/ActionServer.java" + "src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java" + "src/main/java/org/ros2/rcljava/action/ActionServerImpl.java" "src/main/java/org/ros2/rcljava/action/CancelCallback.java" "src/main/java/org/ros2/rcljava/action/CancelResponse.java" "src/main/java/org/ros2/rcljava/action/GoalCallback.java" "src/main/java/org/ros2/rcljava/action/GoalResponse.java" - "src/main/java/org/ros2/rcljava/action/ActionServer.java" - "src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java" - "src/main/java/org/ros2/rcljava/action/ActionServerImpl.java" + "src/main/java/org/ros2/rcljava/action/GoalStatus.java" "src/main/java/org/ros2/rcljava/client/Client.java" "src/main/java/org/ros2/rcljava/client/ClientImpl.java" "src/main/java/org/ros2/rcljava/concurrent/Callback.java" @@ -219,6 +225,8 @@ add_jar("${PROJECT_NAME}_jar" ${builtin_interfaces_JARS} ${rcl_interfaces_JARS} ${rosgraph_msgs_JARS} + ${action_msgs_JARS} + ${unique_identifier_msgs_JARS} ) install_jar("${PROJECT_NAME}_jar" "share/${PROJECT_NAME}/java") @@ -259,6 +267,8 @@ if(BUILD_TESTING) builtin_interfaces rcl_interfaces rosgraph_msgs + action_msgs + unique_identifier_msgs ${_java_type_supports} SKIP_INSTALL ) @@ -276,6 +286,7 @@ if(BUILD_TESTING) "src/test/java/org/ros2/rcljava/SpinTest.java" "src/test/java/org/ros2/rcljava/TimeTest.java" "src/test/java/org/ros2/rcljava/action/ActionServerTest.java" + "src/test/java/org/ros2/rcljava/action/MockActionClient.java" "src/test/java/org/ros2/rcljava/client/ClientTest.java" "src/test/java/org/ros2/rcljava/contexts/ContextTest.java" "src/test/java/org/ros2/rcljava/node/NodeOptionsTest.java" @@ -357,6 +368,24 @@ if(BUILD_TESTING) list_append_unique(_deps_library_dirs ${_dep_dir}) endforeach() + foreach(_dep_lib ${action_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${action_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + + foreach(_dep_lib ${unique_identifier_msgs_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${unique_identifier_msgs_JNI_LIBRARIES}) + get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) + list_append_unique(_deps_library_dirs ${_dep_dir}) + endforeach() + foreach(_dep_lib ${test_msgs_LIBRARIES}) get_filename_component(_dep_dir "${_dep_lib}" DIRECTORY) list_append_unique(_deps_library_dirs ${_dep_dir}) @@ -381,6 +410,7 @@ if(BUILD_TESTING) TESTS "${testsuite}" INCLUDE_JARS + "${action_msgs_JARS}" "${rcljava_common_JARS}" "${rcljava_test_msgs_JARS}" "${std_msgs_JARS}" @@ -389,6 +419,7 @@ if(BUILD_TESTING) "${rosgraph_msgs_JARS}" "${test_msgs_JARS}" "${mockito_vendor_JARS}" + "${unique_identifier_msgs_JARS}" "${_${PROJECT_NAME}_jar_file}" "${_${PROJECT_NAME}_messages_jar_file}" APPEND_LIBRARY_DIRS diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h index dba7e007..53a2832b 100644 --- a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -20,6 +20,52 @@ #ifdef __cplusplus extern "C" { #endif + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetNumberOfSubscriptions + * Signature: (L)I + */ +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfSubscriptions( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetNumberOfTimers + * Signature: (L)I + */ +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfTimers( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetNumberOfClients + * Signature: (L)I + */ +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfClients( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetNumberOfServices + * Signature: (L)I + */ +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfServices( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeGetReadyEntities + * Signature: (LL)[Z + */ +JNIEXPORT jbooleanArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetReadyEntities( + JNIEnv *, jclass, jlong, jlong); + /* * Class: org_ros2_rcljava_action_ActionServerImpl * Method: nativeDispose @@ -37,6 +83,80 @@ JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( JNIEnv *, jobject, jlong, jlong, jclass, jstring); +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeGoalRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeGoalRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeCancelRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeCancelRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeTakeResultRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;)Lorg/ros2/rcljava/RMWRequestId; + */ +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendGoalResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendCancelResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeSendResultResponse + * Signature: (JLorg/ros2/rcljava/RMWRequestId;JJJLorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( + JNIEnv *, jclass, jlong, jobject, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeProcessCancelRequest + * Signature: (JJJJLorg/ros2/rcljava/interfaces/MessageDefinition;Lorg/ros2/rcljava/interfaces/MessageDefinition;) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeProcessCancelRequest( + JNIEnv *, jclass, jlong, jlong, jlong, jlong, jobject, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl + * Method: nativeCheckGoalExists + * Signature: (JLorg/ros2/rcljava/interfaces/MessageDefinition;JJ)Z + */ + +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( + JNIEnv * env, jclass, + jlong, jobject, jlong, jlong); + #ifdef __cplusplus } #endif diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h new file mode 100644 index 00000000..94aaab23 --- /dev/null +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h @@ -0,0 +1,99 @@ +// Copyright 2020 ros2-java contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +/* Header for class org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl */ + +#ifndef ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H_ +#define ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H_ +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeAcceptNewGoal + * Signature: (JJJLorg/ros2/rcljava/interfaces/MessageDefinition;)J + */ +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeAcceptNewGoal( + JNIEnv *, jclass, jlong, jlong, jlong, jobject); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGetStatus + * Signature: (J)I + */ +JNIEXPORT int +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGetStatus( + JNIEnv *, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventExecute + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventCancelGoal + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventSucceed + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventAbort + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeGoalEventCanceled + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( + JNIEnv * env, jclass, jlong); + +/* + * Class: org_ros2_rcljava_action_ActionServerImpl$GoalHandleImpl + * Method: nativeDispose + * Signature: (J) + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeDipose( + JNIEnv *, jclass, jlong); + +#ifdef __cplusplus +} +#endif +#endif // ORG_ROS2_RCLJAVA_ACTION_ACTIONSERVERIMPL_GOALHANDLEIMPL_H__ diff --git a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h index 5522ac8d..e96b88c8 100644 --- a/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h +++ b/rcljava/include/org_ros2_rcljava_executors_BaseExecutor.h @@ -98,6 +98,15 @@ JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddClient( JNIEnv *, jclass, jlong, jlong); +/* + * Class: org_ros2_rcljava_executors_BaseExecutor + * Method: nativeWaitSetAddActionServer + * Signature: (JJ)V + */ +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddActionServer( + JNIEnv *, jclass, jlong, jlong); + /* * Class: org_ros2_rcljava_executors_BaseExecutor * Method: nativeTakeRequest diff --git a/rcljava/package.xml b/rcljava/package.xml index 67ac38d5..ffe81e41 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -12,6 +12,7 @@ ament_cmake_export_jni_libraries rcljava_common + action_msgs builtin_interfaces rcl_interfaces rcl @@ -22,6 +23,7 @@ rosgraph_msgs rosidl_generator_c rosidl_typesupport_c + unique_identifier_msgs builtin_interfaces rcl_interfaces rmw @@ -30,6 +32,7 @@ rosidl_generator_java rosidl_typesupport_c + action_msgs builtin_interfaces rcl_interfaces rcl @@ -40,6 +43,7 @@ rosgraph_msgs rosidl_runtime_c rosidl_parser + unique_identifier_msgs ament_lint_auto ament_lint_common diff --git a/rcljava/src/main/cpp/convert.hpp b/rcljava/src/main/cpp/convert.hpp new file mode 100644 index 00000000..72b64c30 --- /dev/null +++ b/rcljava/src/main/cpp/convert.hpp @@ -0,0 +1,89 @@ +// Copyright 2017-2018 Esteve Fernandez +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +#include + +#include +#include + +#include "rmw/rmw.h" + +#ifndef MAIN__CPP__CONVERT_HPP_ +#define MAIN__CPP__CONVERT_HPP_ +#ifdef __cplusplus +extern "C" { +#endif + +jobject +convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) +{ + jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/service/RMWRequestId"); + assert(jrequest_id_class != nullptr); + + jmethodID jconstructor = env->GetMethodID(jrequest_id_class, "", "()V"); + assert(jconstructor != nullptr); + + jobject jrequest_id = env->NewObject(jrequest_id_class, jconstructor); + + jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); + jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); + + assert(jsequence_number_field_id != nullptr); + assert(jwriter_guid_field_id != nullptr); + + int8_t * writer_guid = request_id->writer_guid; + int64_t sequence_number = request_id->sequence_number; + + env->SetLongField(jrequest_id, jsequence_number_field_id, sequence_number); + + jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h + + jbyteArray jwriter_guid = env->NewByteArray(writer_guid_len); + env->SetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); + env->SetObjectField(jrequest_id, jwriter_guid_field_id, jwriter_guid); + + return jrequest_id; +} + +rmw_request_id_t * +convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) +{ + assert(jrequest_id != nullptr); + + jclass jrequest_id_class = env->GetObjectClass(jrequest_id); + assert(jrequest_id_class != nullptr); + + jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); + jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); + + assert(jsequence_number_field_id != nullptr); + assert(jwriter_guid_field_id != nullptr); + + rmw_request_id_t * request_id = static_cast(malloc(sizeof(rmw_request_id_t))); + + int8_t * writer_guid = request_id->writer_guid; + request_id->sequence_number = env->GetLongField(jrequest_id, jsequence_number_field_id); + + jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h + + jbyteArray jwriter_guid = (jbyteArray)env->GetObjectField(jrequest_id, jwriter_guid_field_id); + env->GetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); + + return request_id; +} + +#ifdef __cplusplus +} +#endif +#endif // MAIN__CPP__CONVERT_HPP_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp index fa52d563..1f9be797 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -21,14 +21,112 @@ #include "rcl/error_handling.h" #include "rcl/rcl.h" #include "rcl_action/rcl_action.h" -#include "rosidl_generator_c/message_type_support_struct.h" +#include "rosidl_runtime_c/message_type_support_struct.h" #include "rcljava_common/exceptions.hpp" #include "rcljava_common/signatures.hpp" #include "org_ros2_rcljava_action_ActionServerImpl.h" +#include "./convert.hpp" + using rcljava_common::exceptions::rcljava_throw_rclexception; +using rcljava_common::signatures::convert_from_java_signature; +using rcljava_common::signatures::convert_to_java_signature; +using rcljava_common::signatures::destroy_ros_message_signature; + +#define RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(Type) \ + do { \ + size_t num_subscriptions; \ + size_t num_guard_conditions; \ + size_t num_timers; \ + size_t num_clients; \ + size_t num_services; \ + rcl_action_server_t * action_server = reinterpret_cast( \ + action_server_handle); \ + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( \ + action_server, \ + &num_subscriptions, \ + &num_guard_conditions, \ + &num_timers, \ + &num_clients, \ + &num_services); \ + if (ret != RCL_RET_OK) { \ + std::string msg = \ + "Failed to get number of entities for an action server: " + \ + std::string(rcl_get_error_string().str); \ + rcl_reset_error(); \ + rcljava_throw_rclexception(env, ret, msg); \ + } \ + return static_cast(num_ ## Type ## s); \ + } \ + while (0) + +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfSubscriptions( + JNIEnv * env, jclass, jlong action_server_handle) +{ + RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(subscription); +} + +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfTimers( + JNIEnv * env, jclass, jlong action_server_handle) +{ + RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(timer); +} + +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfClients( + JNIEnv * env, jclass, jlong action_server_handle) +{ + RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(client); +} + +JNIEXPORT jint +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfServices( + JNIEnv * env, jclass, jlong action_server_handle) +{ + RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(service); +} + +JNIEXPORT jbooleanArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetReadyEntities( + JNIEnv * env, jclass, jlong action_server_handle, jlong wait_set_handle) +{ + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + + bool is_goal_request_ready = false; + bool is_cancel_request_ready = false; + bool is_result_request_ready = false; + bool is_goal_expired = false; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + action_server, + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to get ready entities for action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return NULL; + } + + jbooleanArray result = env->NewBooleanArray(4); + jboolean temp_result[4] = { + is_goal_request_ready, + is_cancel_request_ready, + is_result_request_ready, + is_goal_expired + }; + env->SetBooleanArrayRegion(result, 0, 4, temp_result); + return result; +} JNIEXPORT void JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeDispose( @@ -78,10 +176,10 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( jlong jts = env->CallStaticLongMethod(jaction_class, mid); assert(jts != 0); - const char * action_name = env->GetStringUTFChars(jaction_name, 0); + const char * action_name_tmp = env->GetStringUTFChars(jaction_name, 0); - // std::string service_name(service_name_tmp); - // env->ReleaseStringUTFChars(jservice_name, service_name_tmp); + std::string action_name(action_name_tmp); + env->ReleaseStringUTFChars(jaction_name, action_name_tmp); rcl_node_t * node = reinterpret_cast(node_handle); rcl_clock_t * clock = reinterpret_cast(clock_handle); @@ -94,16 +192,195 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); rcl_ret_t ret = rcl_action_server_init( - action_server, node, clock, ts, action_name, &action_server_ops); - env->ReleaseStringUTFChars(jaction_name, action_name); + action_server, node, clock, ts, action_name.c_str(), &action_server_ops); + // env->ReleaseStringUTFChars(jaction_name, action_name); if (ret != RCL_RET_OK) { std::string msg = "Failed to create action server: " + std::string(rcl_get_error_string().str); rcl_reset_error(); rcljava_throw_rclexception(env, ret, msg); + free(action_server); return 0; } jlong jaction_server = reinterpret_cast(action_server); return jaction_server; } + +#define RCLJAVA_ACTION_SERVER_TAKE_REQUEST(Type) \ + do { \ + assert(jrequest_from_java_converter_handle != 0); \ + assert(jrequest_to_java_converter_handle != 0); \ + rcl_action_server_t * action_server = reinterpret_cast( \ + action_server_handle); \ + convert_from_java_signature convert_from_java = \ + reinterpret_cast(jrequest_from_java_converter_handle); \ + convert_to_java_signature convert_to_java = \ + reinterpret_cast(jrequest_to_java_converter_handle); \ + destroy_ros_message_signature destroy_ros_message = \ + reinterpret_cast(jrequest_destructor_handle); \ + void * taken_msg = convert_from_java(jrequest_msg, nullptr); \ + rmw_request_id_t header; \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _request(action_server, &header, taken_msg); \ + if (ret != RCL_RET_OK && ret != RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + destroy_ros_message(taken_msg); \ + std::string msg = \ + "Failed to take " #Type " request: " + std::string(rcl_get_error_string().str); \ + rcl_reset_error(); \ + rcljava_throw_rclexception(env, ret, msg); \ + return nullptr; \ + } \ + if (RCL_RET_OK == ret) { \ + jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); \ + destroy_ros_message(taken_msg); \ + assert(jtaken_msg != nullptr); \ + jobject jheader = convert_rmw_request_id_to_java(env, &header); \ + return jheader; \ + } \ + destroy_ros_message(taken_msg); \ + return nullptr; \ + } \ + while (0) + +#define RCLJAVA_ACTION_SERVER_SEND_RESPONSE(Type) \ + do { \ + assert(jresponse_from_java_converter_handle != 0); \ + assert(jresponse_to_java_converter_handle != 0); \ + assert(jresponse_destructor_handle != 0); \ + rcl_action_server_t * action_server = reinterpret_cast( \ + action_server_handle); \ + convert_from_java_signature convert_from_java = \ + reinterpret_cast(jresponse_from_java_converter_handle); \ + void * response_msg = convert_from_java(jresponse_msg, nullptr); \ + rmw_request_id_t * request_id = convert_rmw_request_id_from_java(env, jrequest_id); \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ + action_server, request_id, response_msg); \ + destroy_ros_message_signature destroy_ros_message = \ + reinterpret_cast(jresponse_destructor_handle); \ + destroy_ros_message(response_msg); \ + if (ret != RCL_RET_OK) { \ + std::string msg = \ + "Failed to send " #Type " response: " + std::string(rcl_get_error_string().str); \ + rcl_reset_error(); \ + rcljava_throw_rclexception(env, ret, msg); \ + } \ + } \ + while (0) + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeGoalRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(goal); +} + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeCancelRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(cancel); +} + +JNIEXPORT jobject +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeTakeResultRequest( + JNIEnv * env, jclass, jlong action_server_handle, jlong jrequest_from_java_converter_handle, + jlong jrequest_to_java_converter_handle, jlong jrequest_destructor_handle, jobject jrequest_msg) +{ + RCLJAVA_ACTION_SERVER_TAKE_REQUEST(result); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendGoalResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(goal); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendCancelResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(cancel); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeSendResultResponse( + JNIEnv * env, jclass, jlong action_server_handle, jobject jrequest_id, + jlong jresponse_from_java_converter_handle, jlong jresponse_to_java_converter_handle, + jlong jresponse_destructor_handle, jobject jresponse_msg) +{ + RCLJAVA_ACTION_SERVER_SEND_RESPONSE(result); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeProcessCancelRequest( + JNIEnv * env, jclass, + jlong action_server_handle, + jlong jrequest_from_java_converter_handle, + jlong jrequest_destructor_handle, + jlong jresponse_to_java_converter_handle, + jobject jrequest_msg, + jobject jresponse_msg) +{ + assert(jrequest_from_java_converter_handle != 0); + assert(jrequest_destructor_handle != 0); + assert(jresponse_to_java_converter_handle != 0); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + convert_from_java_signature request_convert_from_java = + reinterpret_cast(jrequest_from_java_converter_handle); + convert_to_java_signature response_convert_to_java = reinterpret_cast( + jresponse_to_java_converter_handle); + destroy_ros_message_signature request_destroy_ros_message = + reinterpret_cast(jrequest_destructor_handle); + + rcl_action_cancel_request_t * request_msg = reinterpret_cast( + request_convert_from_java(jrequest_msg, nullptr)); + rcl_action_cancel_response_t response_msg = rcl_action_get_zero_initialized_cancel_response(); + + rcl_ret_t ret = rcl_action_process_cancel_request( + action_server, request_msg, &response_msg); + request_destroy_ros_message(request_msg); + if (ret != RCL_RET_OK) { + std::string msg = \ + "Failed to process cancel request: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return; + } + + response_convert_to_java(&response_msg, jresponse_msg); +} + +JNIEXPORT jboolean +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeCheckGoalExists( + JNIEnv *, jclass, + jlong jaction_server, + jobject jgoal_info, + jlong jgoal_info_from_java_converter_handle, + jlong jgoal_info_destructor_handle) +{ + assert(0 != jgoal_info_from_java_converter_handle); + assert(0 != jgoal_info_destructor_handle); + + rcl_action_server_t * action_server = reinterpret_cast( + jaction_server); + convert_from_java_signature convert_from_java = + reinterpret_cast(jgoal_info_from_java_converter_handle); + destroy_ros_message_signature destroy_ros_message = + reinterpret_cast(jgoal_info_destructor_handle); + + rcl_action_goal_info_t * goal_info = + reinterpret_cast(convert_from_java(jgoal_info, nullptr)); + bool exists = rcl_action_server_goal_exists(action_server, goal_info); + destroy_ros_message(goal_info); + + return exists; +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp new file mode 100644 index 00000000..de1fb4b6 --- /dev/null +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.cpp @@ -0,0 +1,149 @@ +// Copyright 2020 ros2-java contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl_action/rcl_action.h" + +#include "rcljava_common/exceptions.hpp" +#include "rcljava_common/signatures.hpp" + +#include "org_ros2_rcljava_action_ActionServerImpl_GoalHandleImpl.h" + +using rcljava_common::exceptions::rcljava_throw_rclexception; +using rcljava_common::signatures::convert_from_java_signature; +using rcljava_common::signatures::destroy_ros_message_signature; + +JNIEXPORT jlong +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeAcceptNewGoal( + JNIEnv * env, jclass, jlong action_server_handle, + jlong jgoal_info_from_java_converter_handle, jlong jgoal_info_destructor_handle, + jobject jgoal_info_message) +{ + assert(jgoal_info_from_java_converter_handle != 0); + + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + convert_from_java_signature convert_from_java = + reinterpret_cast(jgoal_info_from_java_converter_handle); + destroy_ros_message_signature destroy_ros_message = + reinterpret_cast(jgoal_info_destructor_handle); + + rcl_action_goal_info_t * goal_info_message = + reinterpret_cast(convert_from_java(jgoal_info_message, nullptr)); + + rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal( + action_server, goal_info_message); + destroy_ros_message(goal_info_message); + if (!goal_handle) { + std::string msg = "Failed to accept new goal: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + // '1' is arbitrary since we don't have a specific return code + rcljava_throw_rclexception(env, 1, msg); + return 0; + } + + jlong jgoal_handle = reinterpret_cast(goal_handle); + return jgoal_handle; +} + +JNIEXPORT int +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGetStatus( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + rcl_action_goal_state_t status; + rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to get goal status: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + return static_cast(GOAL_STATE_UNKNOWN); + } + + return static_cast(status); +} + +static void update_goal_state(JNIEnv * env, jlong jgoal_handle, rcl_action_goal_event_t event) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, event); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to update goal state with event: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventExecute( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_EXECUTE); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCancelGoal( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCEL_GOAL); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventSucceed( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_SUCCEED); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventAbort( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_ABORT); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeGoalEventCanceled( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + update_goal_state(env, jgoal_handle, GOAL_EVENT_CANCELED); +} + +JNIEXPORT void +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_00024GoalHandleImpl_nativeDipose( + JNIEnv * env, jclass, jlong jgoal_handle) +{ + rcl_action_goal_handle_t * goal_handle = reinterpret_cast( + jgoal_handle); + if (!goal_handle) { + // Nothing to dispose + return; + } + + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + if (RCL_RET_OK != ret) { + std::string msg = "Failed to finalize goal handle: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp index 287a175f..8682e5b1 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp @@ -22,6 +22,7 @@ #include "rcl/node.h" #include "rcl/rcl.h" #include "rcl/timer.h" +#include "rcl_action/rcl_action.h" #include "rmw/rmw.h" #include "rosidl_runtime_c/message_type_support_struct.h" @@ -30,69 +31,13 @@ #include "org_ros2_rcljava_executors_BaseExecutor.h" +#include "./convert.hpp" + using rcljava_common::exceptions::rcljava_throw_rclexception; using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::convert_to_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; -jobject -convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) -{ - jclass jrequest_id_class = env->FindClass("org/ros2/rcljava/service/RMWRequestId"); - assert(jrequest_id_class != nullptr); - - jmethodID jconstructor = env->GetMethodID(jrequest_id_class, "", "()V"); - assert(jconstructor != nullptr); - - jobject jrequest_id = env->NewObject(jrequest_id_class, jconstructor); - - jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); - jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); - - assert(jsequence_number_field_id != nullptr); - assert(jwriter_guid_field_id != nullptr); - - int8_t * writer_guid = request_id->writer_guid; - int64_t sequence_number = request_id->sequence_number; - - env->SetLongField(jrequest_id, jsequence_number_field_id, sequence_number); - - jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h - - jbyteArray jwriter_guid = env->NewByteArray(writer_guid_len); - env->SetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); - env->SetObjectField(jrequest_id, jwriter_guid_field_id, jwriter_guid); - - return jrequest_id; -} - -rmw_request_id_t * -convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) -{ - assert(jrequest_id != nullptr); - - jclass jrequest_id_class = env->GetObjectClass(jrequest_id); - assert(jrequest_id_class != nullptr); - - jfieldID jsequence_number_field_id = env->GetFieldID(jrequest_id_class, "sequenceNumber", "J"); - jfieldID jwriter_guid_field_id = env->GetFieldID(jrequest_id_class, "writerGUID", "[B"); - - assert(jsequence_number_field_id != nullptr); - assert(jwriter_guid_field_id != nullptr); - - rmw_request_id_t * request_id = static_cast(malloc(sizeof(rmw_request_id_t))); - - int8_t * writer_guid = request_id->writer_guid; - request_id->sequence_number = env->GetLongField(jrequest_id, jsequence_number_field_id); - - jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h - - jbyteArray jwriter_guid = (jbyteArray)env->GetObjectField(jrequest_id, jwriter_guid_field_id); - env->GetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); - - return request_id; -} - JNIEXPORT jlong JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeGetZeroInitializedWaitSet(JNIEnv *, jclass) { @@ -261,6 +206,23 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddClient( } } +JNIEXPORT void JNICALL +Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddActionServer( + JNIEnv * env, jclass, jlong wait_set_handle, jlong action_server_handle) +{ + rcl_wait_set_t * wait_set = reinterpret_cast(wait_set_handle); + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); + if (ret != RCL_RET_OK) { + std::string msg = + "Failed to add action server to wait set: " + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } +} + JNIEXPORT void JNICALL Java_org_ros2_rcljava_executors_BaseExecutor_nativeWaitSetAddTimer( JNIEnv * env, jclass, jlong wait_set_handle, jlong timer_handle) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java index df2615ae..b3ba2082 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java @@ -15,9 +15,57 @@ package org.ros2.rcljava.action; +import java.util.Collection; + import org.ros2.rcljava.interfaces.Disposable; -// import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ActionDefinition; public interface ActionServer extends Disposable { + + /** + * Get the number of underlying subscriptions that the action server uses. + * + * @return The number of subscriptions. + */ + int getNumberOfSubscriptions(); + + /** + * Get the number of underlying timers that the action server uses. + * + * @return The number of timers. + */ + int getNumberOfTimers(); + + /** + * Get the number of underlying clients that the action server uses. + * + * @return The number of clients. + */ + int getNumberOfClients(); + + /** + * Get the number of underlying services that the action server uses. + * + * @return The number of services. + */ + int getNumberOfServices(); + + /** + * Get the goal handles that the action server is currently tracking. + */ + // Collection> getGoalHandles(); + + /** + * Check if an entity of the action server is ready in the wait set. + * + * @param waitSetHandle Handle to the rcl wait set that this action server was added to. + * + * @return true if at least one entity is ready, false otherwise. + */ + boolean isReady(long waitSetHandle); + + /** + * Execute any entities that are ready in the underlying wait set. + */ + void execute(); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java index eff0e5e6..aedf0dbe 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -16,7 +16,48 @@ package org.ros2.rcljava.action; import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.Disposable; +import org.ros2.rcljava.interfaces.MessageDefinition; -public class ActionServerGoalHandle { +public interface ActionServerGoalHandle extends Disposable { + /** + * Get the message containing the timestamp and ID for the goal. + */ + public action_msgs.msg.GoalInfo getGoalInfo(); + /** + * Get the goal message. + */ + public MessageDefinition getGoal(); + + /** + * Get the goal status. + */ + public GoalStatus getGoalStatus(); + + /** + * Returns true if the goal is in the CANCELING state. + */ + public boolean isCanceling(); + + /** + * Transition the goal to the SUCCEEDED state. + * + * Pre-condition: the goal must be in the EXECUTING or CANCELING state. + */ + public void succeed(); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the CANCELING state. + */ + public void canceled(); + + /** + * Transition the goal the the CANCELED state. + * + * Pre-condition: the goal must be in the EXCUTING or CANCELING state. + */ + public void abort(); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 6b5fca45..a65da750 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -16,18 +16,159 @@ package org.ros2.rcljava.action; import java.lang.ref.WeakReference; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.List; +import java.util.Map; import org.ros2.rcljava.RCLJava; import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.consumers.Consumer; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; +import org.ros2.rcljava.interfaces.GoalResponseDefinition; import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.service.RMWRequestId; +import org.ros2.rcljava.time.Clock; +import org.ros2.rcljava.Time; import org.slf4j.Logger; import org.slf4j.LoggerFactory; public class ActionServerImpl implements ActionServer { + static class GoalHandleImpl implements ActionServerGoalHandle { + private static final Logger logger = LoggerFactory.getLogger(GoalHandleImpl.class); + + static { + try { + JNIUtils.loadImplementation(GoalHandleImpl.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); + } + } + + private long handle; + private ActionServer actionServer; + private action_msgs.msg.GoalInfo goalInfo; + private MessageDefinition goal; + + private static native long nativeAcceptNewGoal( + long actionServerHandle, + long goalInfoFromJavaConverterHandle, + long goalInfoDestructorHandle, + MessageDefinition goalInfo); + private static native int nativeGetStatus(long goalHandle); + private static native void nativeGoalEventExecute(long goalHandle); + private static native void nativeGoalEventCancelGoal(long goalHandle); + private static native void nativeGoalEventSucceed(long goalHandle); + private static native void nativeGoalEventAbort(long goalHandle); + private static native void nativeGoalEventCanceled(long goalHandle); + private static native void nativeDispose(long handle); + + public GoalHandleImpl( + ActionServer actionServer, action_msgs.msg.GoalInfo goalInfo, MessageDefinition goal) + { + this.actionServer = actionServer; + this.goalInfo = goalInfo; + this.goal = goal; + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + this.handle = nativeAcceptNewGoal( + actionServer.getHandle(), + goalInfoFromJavaConverterHandle, + goalInfoDestructorHandle, + goalInfo); + } + + /** + * {@inheritDoc} + */ + public synchronized action_msgs.msg.GoalInfo getGoalInfo() { + return this.goalInfo; + } + + /** + * {@inheritDoc} + */ + public synchronized MessageDefinition getGoal() { + return this.goal; + } + + /** + * {@inheritDoc} + */ + public synchronized GoalStatus getGoalStatus() { + int status = nativeGetStatus(this.handle); + return GoalStatus.fromMessageValue((byte)status); + } + + /** + * {@inheritDoc} + */ + public synchronized boolean isCanceling() { + return this.getGoalStatus() == GoalStatus.CANCELING; + } + + /** + * Transition the goal to the EXECUTING state. + */ + public synchronized void execute() { + // It's possible that there has been a request to cancel the goal prior to executing. + // In this case we want to avoid the illegal state transition to EXECUTING + // but still call the users execute callback to let them handle canceling the goal. + if (!this.isCanceling()) { + nativeGoalEventExecute(this.handle); + } + // self._action_server.notify_execute(self, execute_callback) + } + + /** + * Transition the goal to the CANCELING state. + */ + public synchronized void cancelGoal() { + nativeGoalEventCancelGoal(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void succeed() { + nativeGoalEventSucceed(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void canceled() { + nativeGoalEventCanceled(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized void abort() { + nativeGoalEventAbort(this.handle); + } + + /** + * {@inheritDoc} + */ + public synchronized final void dispose() { + nativeDispose(this.handle); + this.handle = 0; + } + + /** + * {@inheritDoc} + */ + public synchronized final long getHandle() { + return handle; + } + } // class GoalHandleImpl + private static final Logger logger = LoggerFactory.getLogger(ActionServerImpl.class); static { @@ -40,12 +181,34 @@ public class ActionServerImpl implements ActionServe } private final WeakReference nodeReference; + private final Clock clock; + private final T actionTypeInstance; private final String actionName; private long handle; - private final GoalCallback goalCallback; + private final GoalCallback goalCallback; private final CancelCallback cancelCallback; private final Consumer> acceptedCallback; + private boolean[] readyEntities; + + private Map, GoalHandleImpl> goalHandles; + + private boolean isGoalRequestReady() { + return this.readyEntities[0]; + } + + private boolean isCancelRequestReady() { + return this.readyEntities[1]; + } + + private boolean isResultRequestReady() { + return this.readyEntities[2]; + } + + private boolean isGoalExpiredReady() { + return this.readyEntities[3]; + } + private native long nativeCreateActionServer( long nodeHandle, long clockHandle, Class cls, String actionName); @@ -63,26 +226,328 @@ public ActionServerImpl( final WeakReference nodeReference, final Class actionType, final String actionName, - final GoalCallback goalCallback, + final GoalCallback goalCallback, final CancelCallback cancelCallback, - final Consumer> acceptedCallback) { + final Consumer> acceptedCallback) throws IllegalArgumentException { this.nodeReference = nodeReference; + try { + this.actionTypeInstance = actionType.newInstance(); + } catch (Exception ex) { + throw new IllegalArgumentException("Failed to instantiate provided action type", ex); + } this.actionName = actionName; this.goalCallback = goalCallback; this.cancelCallback = cancelCallback; this.acceptedCallback = acceptedCallback; + this.goalHandles = new HashMap, GoalHandleImpl>(); + Node node = nodeReference.get(); - if (node != null) { - // TODO: throw + if (node == null) { + throw new IllegalArgumentException("Node reference is null"); } + this.clock = node.getClock(); + this.handle = nativeCreateActionServer( node.getHandle(), node.getClock().getHandle(), actionType, actionName); // TODO(jacobperron): Introduce 'Waitable' interface for entities like timers, services, etc // node.addWaitable(this); } + private static native int nativeGetNumberOfSubscriptions(long handle); + private static native int nativeGetNumberOfTimers(long handle); + private static native int nativeGetNumberOfClients(long handle); + private static native int nativeGetNumberOfServices(long handle); + + /** + * {@inheritDoc} + */ + public int getNumberOfSubscriptions() { + return nativeGetNumberOfSubscriptions(this.handle); + } + + /** + * {@inheritDoc} + */ + public int getNumberOfTimers() { + return nativeGetNumberOfTimers(this.handle); + } + + /** + * {@inheritDoc} + */ + public int getNumberOfClients() { + return nativeGetNumberOfClients(this.handle); + } + + /** + * {@inheritDoc} + */ + public int getNumberOfServices() { + return nativeGetNumberOfServices(this.handle); + } + + private static native boolean[] nativeGetReadyEntities( + long actionServerHandle, long waitSetHandle); + + /** + * {@inheritDoc} + */ + public boolean isReady(long waitSetHandle) { + this.readyEntities = nativeGetReadyEntities(this.handle, waitSetHandle); + for (boolean isReady : this.readyEntities) { + if (isReady) { + return true; + } + } + return false; + } + + /** + * {@inheritDoc} + */ + // public Collection> getGoalHandles() { + // return this.goalHandles.values(); + // } + + private ActionServerGoalHandle executeGoalRequest( + RMWRequestId rmwRequestId, + GoalRequestDefinition requestMessage, + GoalResponseDefinition responseMessage) + { + builtin_interfaces.msg.Time timeRequestHandled = this.clock.now().toMsg(); + responseMessage.setStamp(timeRequestHandled.getSec(), timeRequestHandled.getNanosec()); + + // Create and populate a GoalInfo message + List goalUuid = requestMessage.getGoalUuid(); + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + unique_identifier_msgs.msg.UUID uuidMessage= new unique_identifier_msgs.msg.UUID(); + uuidMessage.setUuid(goalUuid); + goalInfo.setGoalId(uuidMessage); + goalInfo.setStamp(timeRequestHandled); + + long goalInfoFromJavaConverterHandle = goalInfo.getFromJavaConverterInstance(); + long goalInfoDestructorHandle = goalInfo.getDestructorInstance(); + + // Check that the goal ID isn't already being used + boolean goalExists = nativeCheckGoalExists( + this.handle, goalInfo, goalInfoFromJavaConverterHandle, goalInfoDestructorHandle); + if (goalExists) { + logger.warn("Received goal request for goal already being tracked by action server. Goal ID: " + goalUuid); + responseMessage.accept(false); + return null; + } + + // Workaround type + GoalCallback callback = ((ActionServerImpl) this).goalCallback; + // Call user callback + GoalResponse response = callback.handleGoal(requestMessage); + + boolean accepted = GoalResponse.ACCEPT == response; + responseMessage.accept(accepted); + + System.out.println("Goal request handled " + accepted); + if (!accepted) { + return null; + } + + // Create a goal handle and add it to the list of goals + GoalHandleImpl goalHandle = new GoalHandleImpl( + this, goalInfo, requestMessage.getGoal()); + this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); + return goalHandle; + } + + private action_msgs.srv.CancelGoal_Response executeCancelRequest( + action_msgs.srv.CancelGoal_Response inputMessage) + { + action_msgs.srv.CancelGoal_Response outputMessage = new action_msgs.srv.CancelGoal_Response(); + outputMessage.setReturnCode(inputMessage.getReturnCode()); + List goalsToCancel = new ArrayList(); + + logger.warn("Proposed number of goals to cancel " + inputMessage.getGoalsCanceling().size()); + // Process user callback for each goal in cancel request + for (action_msgs.msg.GoalInfo goalInfo : inputMessage.getGoalsCanceling()) { + List goalUuid = goalInfo.getGoalId().getUuid(); + // It's possible a goal may not be tracked by the user + if (!this.goalHandles.containsKey(goalUuid)) { + logger.warn("Ignoring cancel request for untracked goal handle with ID '" + goalUuid + "'"); + continue; + } + GoalHandleImpl goalHandle = this.goalHandles.get(goalUuid); + CancelResponse cancelResponse = this.cancelCallback.handleCancel(goalHandle); + + if (CancelResponse.ACCEPT == cancelResponse) { + // Update goal state to CANCELING + goalHandle.cancelGoal(); + + // Add to returned response + goalsToCancel.add(goalInfo); + } + } + + logger.warn("number of goals to cancel " + goalsToCancel.size()); + outputMessage.setGoalsCanceling(goalsToCancel); + return outputMessage; + } + + private static native RMWRequestId nativeTakeGoalRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native RMWRequestId nativeTakeCancelRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native RMWRequestId nativeTakeResultRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestToJavaConverterHandle, + long requestDestructorHandle, + MessageDefinition requestMessage); + + private static native void nativeSendGoalResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendCancelResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeSendResultResponse( + long actionServerHandle, + RMWRequestId header, + long responseFromJavaConverterHandle, + long responseToJavaConverterHandle, + long responseDestructorHandle, + MessageDefinition responseMessage); + + private static native void nativeProcessCancelRequest( + long actionServerHandle, + long requestFromJavaConverterHandle, + long requestDestructorHandle, + long responseToJavaConverterHandle, + MessageDefinition requestMessage, + MessageDefinition responseMessage); + + private static native boolean nativeCheckGoalExists( + long handle, + MessageDefinition goalInfo, + long goalInfoFromJavaConverterHandle, + long goalInfoDestructorHandle); + + /** + * {@inheritDoc} + */ + public void execute() { + if (this.isGoalRequestReady()) { + logger.warn("Goal request is ready"); + Class requestType = this.actionTypeInstance.getSendGoalRequestType(); + Class responseType = this.actionTypeInstance.getSendGoalResponseType(); + + GoalRequestDefinition requestMessage = null; + GoalResponseDefinition responseMessage = null; + + try { + requestMessage = requestType.newInstance(); + responseMessage = responseType.newInstance(); + } catch (InstantiationException ie) { + ie.printStackTrace(); + } catch (IllegalAccessException iae) { + iae.printStackTrace(); + } + + if (requestMessage != null && responseMessage != null) { + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); + long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); + long responseDestructorHandle = responseMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeGoalRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + if (rmwRequestId != null) { + logger.warn("Goal request taken, executing user callback"); + ActionServerGoalHandle goalHandle = this.executeGoalRequest( + rmwRequestId, requestMessage, responseMessage); + nativeSendGoalResponse( + this.handle, rmwRequestId, + responseFromJavaConverterHandle, responseToJavaConverterHandle, + responseDestructorHandle, responseMessage); + if (goalHandle != null) { + logger.warn("Goal accepted, executing user callback"); + this.acceptedCallback.accept(goalHandle); + } + } + } + } + + if (this.isCancelRequestReady()) { + logger.warn("Cancel request is ready"); + action_msgs.srv.CancelGoal_Request requestMessage = new action_msgs.srv.CancelGoal_Request(); + action_msgs.srv.CancelGoal_Response responseMessage = new action_msgs.srv.CancelGoal_Response(); + + long requestFromJavaConverterHandle = requestMessage.getFromJavaConverterInstance(); + long requestToJavaConverterHandle = requestMessage.getToJavaConverterInstance(); + long requestDestructorHandle = requestMessage.getDestructorInstance(); + long responseFromJavaConverterHandle = responseMessage.getFromJavaConverterInstance(); + long responseToJavaConverterHandle = responseMessage.getToJavaConverterInstance(); + long responseDestructorHandle = responseMessage.getDestructorInstance(); + + RMWRequestId rmwRequestId = + nativeTakeCancelRequest( + this.handle, + requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, + requestMessage); + if (rmwRequestId != null) { + logger.warn("Cancel request taken"); + nativeProcessCancelRequest( + this.handle, + requestFromJavaConverterHandle, + requestDestructorHandle, + responseToJavaConverterHandle, + requestMessage, + responseMessage); + logger.warn("executing user callback"); + responseMessage = executeCancelRequest(responseMessage); + logger.warn("Sending cancel response"); + nativeSendCancelResponse( + this.handle, rmwRequestId, + responseFromJavaConverterHandle, responseToJavaConverterHandle, + responseDestructorHandle, responseMessage); + } + } + + if (this.isResultRequestReady()) { + // executeResultRequest(rmwRequestId, requestMessage, responseMessage); + // TODO + } + + if (this.isGoalExpiredReady()) { + // cleanupExpiredGoals(); + // TODO + } + } + /** * Destroy the underlying rcl_action_server_t. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java new file mode 100644 index 00000000..aee9e1c9 --- /dev/null +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalStatus.java @@ -0,0 +1,47 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +public enum GoalStatus { + UNKNOWN, + ACCEPTED, + EXECUTING, + CANCELING, + SUCCEEDED, + CANCELED, + ABORTED; + + public static GoalStatus fromMessageValue(byte status) { + switch (status) { + case action_msgs.msg.GoalStatus.STATUS_ACCEPTED: + return GoalStatus.ACCEPTED; + case action_msgs.msg.GoalStatus.STATUS_EXECUTING: + return GoalStatus.EXECUTING; + case action_msgs.msg.GoalStatus.STATUS_CANCELING: + return GoalStatus.CANCELING; + case action_msgs.msg.GoalStatus.STATUS_SUCCEEDED: + return GoalStatus.SUCCEEDED; + case action_msgs.msg.GoalStatus.STATUS_CANCELED: + return GoalStatus.CANCELED; + case action_msgs.msg.GoalStatus.STATUS_ABORTED: + return GoalStatus.ABORTED; + case action_msgs.msg.GoalStatus.STATUS_UNKNOWN: + default: + return GoalStatus.UNKNOWN; + } + } +} + diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java b/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java index dd3d3039..701d551b 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/AnyExecutable.java @@ -15,6 +15,7 @@ package org.ros2.rcljava.executors; +import org.ros2.rcljava.action.ActionServer; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.subscription.Subscription; @@ -27,4 +28,5 @@ public class AnyExecutable { public Service service; public Client client; public EventHandler eventHandler; + public ActionServer actionServer; } diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index 67c15fda..f180cf56 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -30,11 +30,13 @@ import org.slf4j.LoggerFactory; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.action.ActionServer; import org.ros2.rcljava.client.Client; import org.ros2.rcljava.common.JNIUtils; import org.ros2.rcljava.events.EventHandler; import org.ros2.rcljava.executors.AnyExecutable; import org.ros2.rcljava.executors.Executor; +import org.ros2.rcljava.interfaces.ActionDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.node.ComposableNode; @@ -69,6 +71,8 @@ public class BaseExecutor { private List> eventHandles = new ArrayList>(); + private List> actionServerHandles = new ArrayList>(); + protected void addNode(ComposableNode node) { this.nodes.add(node); } @@ -168,6 +172,11 @@ protected void executeAnyExecutable(AnyExecutable anyExecutable) { anyExecutable.eventHandler.executeCallback(); eventHandles.remove(anyExecutable.eventHandler.getHandle()); } + + if (anyExecutable.actionServer != null) { + anyExecutable.actionServer.execute(); + this.actionServerHandles.remove(anyExecutable.actionServer.getHandle()); + } } protected void waitForWork(long timeout) { @@ -176,6 +185,7 @@ protected void waitForWork(long timeout) { this.serviceHandles.clear(); this.clientHandles.clear(); this.eventHandles.clear(); + this.actionServerHandles.clear(); for (ComposableNode node : this.nodes) { for (Subscription subscription : node.getNode().getSubscriptions()) { @@ -209,6 +219,11 @@ protected void waitForWork(long timeout) { this.clientHandles.add( new AbstractMap.SimpleEntry(client.getHandle(), client)); } + + for (ActionServer actionServer : node.getNode().getActionServers()) { + this.actionServerHandles.add( + new AbstractMap.SimpleEntry(actionServer.getHandle(), actionServer)); + } } int subscriptionsSize = 0; @@ -222,6 +237,13 @@ protected void waitForWork(long timeout) { timersSize += node.getNode().getTimers().size(); clientsSize += node.getNode().getClients().size(); servicesSize += node.getNode().getServices().size(); + + for (ActionServer actionServer : node.getNode().getActionServers()) { + subscriptionsSize += actionServer.getNumberOfSubscriptions(); + timersSize += actionServer.getNumberOfTimers(); + clientsSize += actionServer.getNumberOfClients(); + servicesSize += actionServer.getNumberOfServices(); + } } if (subscriptionsSize == 0 && timersSize == 0 && clientsSize == 0 && servicesSize == 0) { @@ -256,6 +278,10 @@ protected void waitForWork(long timeout) { nativeWaitSetAddEvent(waitSetHandle, entry.getKey()); } + for (Map.Entry entry : this.actionServerHandles) { + nativeWaitSetAddActionServer(waitSetHandle, entry.getKey()); + } + nativeWait(waitSetHandle, timeout); for (int i = 0; i < this.subscriptionHandles.size(); ++i) { @@ -288,6 +314,12 @@ protected void waitForWork(long timeout) { } } + for (Map.Entry entry : this.actionServerHandles) { + if (!entry.getValue().isReady(waitSetHandle)) { + entry.setValue(null); + } + } + Iterator> subscriptionIterator = this.subscriptionHandles.iterator(); while (subscriptionIterator.hasNext()) { @@ -329,6 +361,14 @@ protected void waitForWork(long timeout) { } } + Iterator> actionServerIterator = this.actionServerHandles.iterator(); + while (actionServerIterator.hasNext()) { + Map.Entry entry = actionServerIterator.next(); + if (entry.getValue() == null) { + actionServerIterator.remove(); + } + } + nativeDisposeWaitSet(waitSetHandle); } @@ -378,6 +418,14 @@ protected AnyExecutable getNextExecutable() { } } + for (Map.Entry entry : this.actionServerHandles) { + if (entry.getValue() != null) { + anyExecutable.actionServer = entry.getValue(); + entry.setValue(null); + return anyExecutable; + } + } + return null; } @@ -461,6 +509,8 @@ private static native MessageDefinition nativeTake( private static native void nativeWaitSetAddEvent(long waitSetHandle, long eventHandle); + private static native void nativeWaitSetAddActionServer(long waitSetHandle, long actionServerHandle); + private static native RMWRequestId nativeTakeRequest(long serviceHandle, long requestFromJavaConverterHandle, long requestToJavaConverterHandle, long requestDestructorHandle, MessageDefinition requestMessage); @@ -482,4 +532,6 @@ private static native RMWRequestId nativeTakeResponse(long clientHandle, private static native boolean nativeWaitSetServiceIsReady(long waitSetHandle, long index); private static native boolean nativeWaitSetClientIsReady(long waitSetHandle, long index); + + private static native boolean nativeWaitSetActionServerIsReady(long waitSetHandle, long actionServerHandle); } diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index 1c47b75f..a486c121 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -33,6 +33,7 @@ import org.ros2.rcljava.graph.NodeNameInfo; import org.ros2.rcljava.interfaces.Disposable; import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.parameters.ParameterCallback; @@ -145,7 +146,7 @@ Client createClient(final Class serviceType, ActionServer createActionServer(final Class actionType, final String actionName, - final GoalCallback goalCallback, + final GoalCallback goalCallback, final CancelCallback cancelCallback, final Consumer> acceptedCallback); diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java index 7d44726e..914b369e 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -33,6 +33,7 @@ import org.ros2.rcljava.graph.NameAndTypes; import org.ros2.rcljava.interfaces.Disposable; import org.ros2.rcljava.interfaces.ActionDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; import org.ros2.rcljava.interfaces.MessageDefinition; import org.ros2.rcljava.interfaces.ServiceDefinition; import org.ros2.rcljava.node.NodeOptions; @@ -402,9 +403,9 @@ private static native long nativeCreateClientHandl public ActionServer createActionServer(final Class actionType, final String actionName, - final GoalCallback goalCallback, + final GoalCallback goalCallback, final CancelCallback cancelCallback, - final Consumer> acceptedCallback) { + final Consumer> acceptedCallback) throws IllegalArgumentException { ActionServer actionServer = new ActionServerImpl( new WeakReference(this), actionType, actionName, goalCallback, cancelCallback, acceptedCallback); diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java index f1f14c86..7a4227f9 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -15,49 +15,290 @@ package org.ros2.rcljava.action; +import java.time.Duration; +import java.util.concurrent.Future; +import java.util.concurrent.TimeUnit; +import java.util.Collection; +import java.util.List; + import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; +import org.junit.After; +import org.junit.AfterClass; +import org.junit.Before; +import org.junit.BeforeClass; import org.junit.Test; import org.ros2.rcljava.RCLJava; +import org.ros2.rcljava.executors.SingleThreadedExecutor; import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.ComposableNode; import org.ros2.rcljava.node.Node; public class ActionServerTest { - @Test - public final void testCreateAndDispose() { + class MockGoalCallback implements GoalCallback { + public test_msgs.action.Fibonacci_Goal goal; + public GoalResponse handleGoal(test_msgs.action.Fibonacci.SendGoalRequest goal) { + this.goal = goal.getGoal(); + return GoalResponse.ACCEPT; + } + } + + class MockCancelCallback implements CancelCallback { + public ActionServerGoalHandle goalHandle; + public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { + System.out.println("Mock goal cancel callback called!"); + this.goalHandle = goalHandle; + return CancelResponse.ACCEPT; + } + } + + class MockAcceptedCallback implements Consumer> { + public ActionServerGoalHandle goalHandle; + public void accept(final ActionServerGoalHandle goalHandle) { + this.goalHandle = goalHandle; + } + } + + private SingleThreadedExecutor executor; + private Node node; + private ComposableNode composableNode; + private ActionServer actionServer; + private MockGoalCallback goalCallback; + private MockCancelCallback cancelCallback; + private MockAcceptedCallback acceptedCallback; + private MockActionClient mockActionClient; + + @BeforeClass + public static void setupOnce() { RCLJava.rclJavaInit(); - Node node = RCLJava.createNode("test_node"); - Consumer> acceptedCallback = - new Consumer>() { - public void accept(final ActionServerGoalHandle goalHandle) {} + org.apache.log4j.BasicConfigurator.configure(); + } + + @AfterClass + public static void tearDownOnce() { + RCLJava.shutdown(); + } + + @Before + public void setUp() throws Exception { + // Create a node + node = RCLJava.createNode("test_action_server_node"); + + assertNotEquals(null, node); + + // Executor requires a ComposableNode type + composableNode = new ComposableNode() { + public Node getNode() { + return node; + } }; + executor = new SingleThreadedExecutor(); + executor.addNode(composableNode); + + // Create action server callbacks + goalCallback = new MockGoalCallback(); + cancelCallback = new MockCancelCallback(); + acceptedCallback = new MockAcceptedCallback(); + + // Create an action server + actionServer = node.createActionServer( + test_msgs.action.Fibonacci.class, "test_action", + goalCallback, cancelCallback, acceptedCallback); + + // Create mock client + mockActionClient = new MockActionClient(node, "test_action"); - ActionServer actionServer = - node.createActionServer( - test_msgs.action.Fibonacci.class, "test_action", - new GoalCallback() { - public GoalResponse handleGoal(test_msgs.action.Fibonacci_Goal goal) { - return GoalResponse.ACCEPT; - } - }, - new CancelCallback() { - public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { - return CancelResponse.REJECT; - } - }, - acceptedCallback); - - assertNotEquals(0, actionServer.getHandle()); - assertEquals(1, node.getActionServers().size()); + // Wait for mock client to discover action sever + // TODO(jacobperron): also wait for action server to discover the client + assertEquals(true, mockActionClient.waitForActionServer(Duration.ofSeconds(5))); + } + @After + public void tearDown() { // We expect that calling dispose should result in a zero handle // and the reference is dropped from the Node actionServer.dispose(); assertEquals(0, actionServer.getHandle()); - assertEquals(0, node.getActionServers().size()); + assertEquals(0, this.node.getActionServers().size()); - RCLJava.shutdown(); + mockActionClient.dispose(); + + executor.removeNode(composableNode); + + node.dispose(); + } + + public test_msgs.action.Fibonacci_SendGoal_Response sendGoal(int order) throws Exception { + test_msgs.action.Fibonacci_SendGoal_Request request = + new test_msgs.action.Fibonacci_SendGoal_Request(); + test_msgs.action.Fibonacci_Goal goal = new test_msgs.action.Fibonacci_Goal(); + goal.setOrder(order); + request.setGoal(goal); + + Future future = + this.mockActionClient.sendGoalClient.asyncSendRequest(request); + + System.out.println("ASync request sent"); + test_msgs.action.Fibonacci_SendGoal_Response response = null; + long startTime = System.nanoTime(); + while (RCLJava.ok() && !future.isDone()) { + System.out.println("spin"); + this.executor.spinOnce(1); + System.out.println("get"); + response = future.get(100, TimeUnit.MILLISECONDS); + + // Check for timeout + long duration = System.nanoTime() - startTime; + System.out.println("Checking for timeout: " + duration / 1000000000); + if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { + break; + } + } + return response; + } + + @Test + public final void testCreateAndDispose() { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Assert no callbacks triggered + assertEquals(null, this.goalCallback.goal); + assertEquals(null, this.cancelCallback.goalHandle); + assertEquals(null, this.acceptedCallback.goalHandle); + } + + @Test + public final void testAcceptGoal() throws Exception { + System.out.println("TEST START accept goal test"); + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Send a goal + test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); + + System.out.println("TEST goal response received"); + assertNotEquals(null, response); + + // Assert goal callback and accepted callback triggered + assertNotEquals(null, this.goalCallback.goal); + assertNotEquals(null, this.acceptedCallback.goalHandle); + + assertEquals(42, this.goalCallback.goal.getOrder()); + test_msgs.action.Fibonacci_Goal acceptedGoal = (test_msgs.action.Fibonacci_Goal)this.acceptedCallback.goalHandle.getGoal(); + assertEquals(42, acceptedGoal.getOrder()); + + // Goal handle should be in action server's list + // Collection> goalHandles = this.actionServer.getGoalHandles(); + // assertEquals(1, goalHandles.size()); + // test_msgs.action.Fibonacci_Goal serverGoal = (test_msgs.action.Fibonacci_Goal)goalHandles.iterator().next().getGoal(); + // assertEquals(42, serverGoal.getOrder()); + + // Assert cancel callback not triggered + assertEquals(null, this.cancelCallback.goalHandle); + + System.out.println("TEST END accept goal test"); + } + + @Test + public final void testCancelGoal() throws Exception { + System.out.println("TEST START cancel test"); + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + System.out.println("TEST Sending goal"); + // Send a goal + test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); + + assertNotEquals(null, response); + + // Goal handle should be in action server's list + // Collection> goalHandles = this.actionServer.getGoalHandles(); + // assertEquals(1, goalHandles.size()); + // test_msgs.action.Fibonacci_Goal serverGoal = (test_msgs.action.Fibonacci_Goal)goalHandles.iterator().next().getGoal(); + // assertEquals(42, serverGoal.getOrder()); + + System.out.println("TEST Canceling goal"); + // Cancel the goal + action_msgs.srv.CancelGoal_Request cancelRequest = new action_msgs.srv.CancelGoal_Request(); + // A zero GoalInfo means cancel all goals + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + unique_identifier_msgs.msg.UUID zeroGoalId = new unique_identifier_msgs.msg.UUID(); + // TODO(jacobperron): code generator should zero initialize to 16 elements + zeroGoalId.setUuid(new byte[16]); + goalInfo.setGoalId(zeroGoalId); + cancelRequest.setGoalInfo(goalInfo); + Future cancelResponseFuture = + this.mockActionClient.cancelGoalClient.asyncSendRequest(cancelRequest); + + System.out.println("TEST Waiting for cancel response"); + // Wait for cancel response + long startTime = System.nanoTime(); + while (RCLJava.ok() && !cancelResponseFuture.isDone()) { + this.executor.spinOnce(100000000); // timeout of 100 milliseconds + + // Check for timeout + long duration = System.nanoTime() - startTime; + if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { + break; + } + } + + System.out.println("TEST cancel response received"); + assertEquals(true, cancelResponseFuture.isDone()); + action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); + List goalsCanceling = cancelResponse.getGoalsCanceling(); + assertEquals(1, goalsCanceling.size()); + + // Assert cancel callback was triggered + assertNotEquals(null, this.cancelCallback.goalHandle); + test_msgs.action.Fibonacci_Goal cancelingGoal = (test_msgs.action.Fibonacci_Goal)this.cancelCallback.goalHandle.getGoal(); + assertEquals(42, cancelingGoal.getOrder()); + + System.out.println("TEST END cancel test"); + } + /* + @Test + public final void testGetResult() throws Exception { + assertNotEquals(0, this.actionServer.getHandle()); + assertEquals(1, this.node.getActionServers().size()); + + // Send a goal + test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); + + assertNotEquals(null, response); + + // Request the result + test_msgs.srv.GetResult_Request getResultRequest = new test_msgs.srv.GetResult_Request(); + //TODO + // getResultRequest. + action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); + cancelRequest.setGoalInfo(goalInfo); + Future cancelResponseFuture = + this.mockActionClient.cancelGoalClient.asyncSendRequest(cancelRequest); + + // Wait for cancel response + long startTime = System.nanoTime(); + while (RCLJava.ok() && !cancelResponseFuture.isDone()) { + this.executor.spinOnce(0); + + // Check for timeout + long duration = System.nanoTime() - startTime; + if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { + break; + } + } + assertEquals(true, cancelResponseFuture.isDone()); + action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); + List goalsCanceling = cancelResponse.getGoalsCanceling(); + assertEquals(1, goalsCanceling.size()); + + // Assert cancel callback was triggered + assertNotEquals(null, this.cancelCallback.goalHandle); + test_msgs.action.Fibonacci_Goal cancelingGoal = (test_msgs.action.Fibonacci_Goal)this.cancelCallback.goalHandle.getGoal(); + assertEquals(42, cancelingGoal.getOrder()); } + */ } diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java new file mode 100644 index 00000000..72f0e7e4 --- /dev/null +++ b/rcljava/src/test/java/org/ros2/rcljava/action/MockActionClient.java @@ -0,0 +1,104 @@ +/* Copyright 2020 ros2-java contributors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.ros2.rcljava.action; + +import java.time.Duration; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +import org.ros2.rcljava.client.Client; +import org.ros2.rcljava.consumers.Consumer; +import org.ros2.rcljava.node.Node; +import org.ros2.rcljava.subscription.Subscription; + +public class MockActionClient { + class FeedbackCallback implements Consumer { + public List feedbackReceived; + + public FeedbackCallback() { + feedbackReceived = Collections.synchronizedList( + new ArrayList()); + } + + public void accept(final test_msgs.action.Fibonacci_Feedback feedback) { + this.feedbackReceived.add(feedback); + } + } + + class StatusCallback implements Consumer { + private action_msgs.msg.GoalStatusArray statusArray; + + public synchronized action_msgs.msg.GoalStatusArray getStatusArrayMessage() { + return this.statusArray; + } + + public synchronized void accept(final action_msgs.msg.GoalStatusArray statusArray) { + this.statusArray = statusArray; + } + } + + public Client sendGoalClient; + public Client getResultClient; + public Client cancelGoalClient; + public FeedbackCallback feedbackCallback; + public StatusCallback statusCallback; + public Subscription feedbackSubscription; + public Subscription statusSubscription; + + public MockActionClient(Node node, String actionName) throws IllegalAccessException, NoSuchFieldException { + // Create mock service clients that make up an action client + sendGoalClient = node.createClient( + test_msgs.action.Fibonacci_SendGoal.class, actionName + "/_action/send_goal"); + getResultClient = node.createClient( + test_msgs.action.Fibonacci_GetResult.class, actionName + "/_action/get_result"); + cancelGoalClient = node.createClient( + action_msgs.srv.CancelGoal.class, actionName + "/_action/cancel_goal"); + // Create mock subscriptions that make up an action client + feedbackCallback = new FeedbackCallback(); + statusCallback = new StatusCallback(); + feedbackSubscription = node.createSubscription( + test_msgs.action.Fibonacci_Feedback.class, + actionName + "/_action/feedback", + feedbackCallback); + statusSubscription = node.createSubscription( + action_msgs.msg.GoalStatusArray.class, + actionName + "/_action/status", + statusCallback); + } + + public void dispose() { + this.sendGoalClient.dispose(); + this.getResultClient.dispose(); + this.cancelGoalClient.dispose(); + this.feedbackSubscription.dispose(); + this.statusSubscription.dispose(); + } + + public boolean waitForActionServer(Duration timeout) { + if (!this.sendGoalClient.waitForService(timeout)) { + return false; + } + if (!this.getResultClient.waitForService(timeout)) { + return false; + } + if (!this.cancelGoalClient.waitForService(timeout)) { + return false; + } + //TODO(jacobperron): wait for feedback and status subscriptions to match publishers, when API is available + return true; + } +} diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java index ee3fe8d3..37c30673 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -1,4 +1,4 @@ -/* Copyright 2016-2018 Esteve Fernandez +/* Cppyright 2016-2018 Esteve Fernandez * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From 0494903042df03e7c982da8e35a3cbb5579bbfbd Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 13 Nov 2020 09:43:04 -0800 Subject: [PATCH 12/32] cleanup Signed-off-by: Jacob Perron --- .../ros2/rcljava/action/ActionServerImpl.java | 9 ------ .../ros2/rcljava/action/ActionServerTest.java | 28 ------------------- 2 files changed, 37 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index a65da750..b3229208 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -366,7 +366,6 @@ private action_msgs.srv.CancelGoal_Response executeCancelRequest( outputMessage.setReturnCode(inputMessage.getReturnCode()); List goalsToCancel = new ArrayList(); - logger.warn("Proposed number of goals to cancel " + inputMessage.getGoalsCanceling().size()); // Process user callback for each goal in cancel request for (action_msgs.msg.GoalInfo goalInfo : inputMessage.getGoalsCanceling()) { List goalUuid = goalInfo.getGoalId().getUuid(); @@ -387,7 +386,6 @@ private action_msgs.srv.CancelGoal_Response executeCancelRequest( } } - logger.warn("number of goals to cancel " + goalsToCancel.size()); outputMessage.setGoalsCanceling(goalsToCancel); return outputMessage; } @@ -456,7 +454,6 @@ private static native boolean nativeCheckGoalExists( */ public void execute() { if (this.isGoalRequestReady()) { - logger.warn("Goal request is ready"); Class requestType = this.actionTypeInstance.getSendGoalRequestType(); Class responseType = this.actionTypeInstance.getSendGoalResponseType(); @@ -486,7 +483,6 @@ public void execute() { requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, requestMessage); if (rmwRequestId != null) { - logger.warn("Goal request taken, executing user callback"); ActionServerGoalHandle goalHandle = this.executeGoalRequest( rmwRequestId, requestMessage, responseMessage); nativeSendGoalResponse( @@ -494,7 +490,6 @@ public void execute() { responseFromJavaConverterHandle, responseToJavaConverterHandle, responseDestructorHandle, responseMessage); if (goalHandle != null) { - logger.warn("Goal accepted, executing user callback"); this.acceptedCallback.accept(goalHandle); } } @@ -502,7 +497,6 @@ public void execute() { } if (this.isCancelRequestReady()) { - logger.warn("Cancel request is ready"); action_msgs.srv.CancelGoal_Request requestMessage = new action_msgs.srv.CancelGoal_Request(); action_msgs.srv.CancelGoal_Response responseMessage = new action_msgs.srv.CancelGoal_Response(); @@ -519,7 +513,6 @@ public void execute() { requestFromJavaConverterHandle, requestToJavaConverterHandle, requestDestructorHandle, requestMessage); if (rmwRequestId != null) { - logger.warn("Cancel request taken"); nativeProcessCancelRequest( this.handle, requestFromJavaConverterHandle, @@ -527,9 +520,7 @@ public void execute() { responseToJavaConverterHandle, requestMessage, responseMessage); - logger.warn("executing user callback"); responseMessage = executeCancelRequest(responseMessage); - logger.warn("Sending cancel response"); nativeSendCancelResponse( this.handle, rmwRequestId, responseFromJavaConverterHandle, responseToJavaConverterHandle, diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java index 7a4227f9..b6566643 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -48,7 +48,6 @@ public GoalResponse handleGoal(test_msgs.action.Fibonacci.SendGoalRequest goal) class MockCancelCallback implements CancelCallback { public ActionServerGoalHandle goalHandle; public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { - System.out.println("Mock goal cancel callback called!"); this.goalHandle = goalHandle; return CancelResponse.ACCEPT; } @@ -140,18 +139,14 @@ public test_msgs.action.Fibonacci_SendGoal_Response sendGoal(int order) throws E Future future = this.mockActionClient.sendGoalClient.asyncSendRequest(request); - System.out.println("ASync request sent"); test_msgs.action.Fibonacci_SendGoal_Response response = null; long startTime = System.nanoTime(); while (RCLJava.ok() && !future.isDone()) { - System.out.println("spin"); this.executor.spinOnce(1); - System.out.println("get"); response = future.get(100, TimeUnit.MILLISECONDS); // Check for timeout long duration = System.nanoTime() - startTime; - System.out.println("Checking for timeout: " + duration / 1000000000); if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { break; } @@ -172,14 +167,12 @@ public final void testCreateAndDispose() { @Test public final void testAcceptGoal() throws Exception { - System.out.println("TEST START accept goal test"); assertNotEquals(0, this.actionServer.getHandle()); assertEquals(1, this.node.getActionServers().size()); // Send a goal test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); - System.out.println("TEST goal response received"); assertNotEquals(null, response); // Assert goal callback and accepted callback triggered @@ -190,37 +183,20 @@ public final void testAcceptGoal() throws Exception { test_msgs.action.Fibonacci_Goal acceptedGoal = (test_msgs.action.Fibonacci_Goal)this.acceptedCallback.goalHandle.getGoal(); assertEquals(42, acceptedGoal.getOrder()); - // Goal handle should be in action server's list - // Collection> goalHandles = this.actionServer.getGoalHandles(); - // assertEquals(1, goalHandles.size()); - // test_msgs.action.Fibonacci_Goal serverGoal = (test_msgs.action.Fibonacci_Goal)goalHandles.iterator().next().getGoal(); - // assertEquals(42, serverGoal.getOrder()); - // Assert cancel callback not triggered assertEquals(null, this.cancelCallback.goalHandle); - - System.out.println("TEST END accept goal test"); } @Test public final void testCancelGoal() throws Exception { - System.out.println("TEST START cancel test"); assertNotEquals(0, this.actionServer.getHandle()); assertEquals(1, this.node.getActionServers().size()); - System.out.println("TEST Sending goal"); // Send a goal test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); assertNotEquals(null, response); - // Goal handle should be in action server's list - // Collection> goalHandles = this.actionServer.getGoalHandles(); - // assertEquals(1, goalHandles.size()); - // test_msgs.action.Fibonacci_Goal serverGoal = (test_msgs.action.Fibonacci_Goal)goalHandles.iterator().next().getGoal(); - // assertEquals(42, serverGoal.getOrder()); - - System.out.println("TEST Canceling goal"); // Cancel the goal action_msgs.srv.CancelGoal_Request cancelRequest = new action_msgs.srv.CancelGoal_Request(); // A zero GoalInfo means cancel all goals @@ -233,7 +209,6 @@ public final void testCancelGoal() throws Exception { Future cancelResponseFuture = this.mockActionClient.cancelGoalClient.asyncSendRequest(cancelRequest); - System.out.println("TEST Waiting for cancel response"); // Wait for cancel response long startTime = System.nanoTime(); while (RCLJava.ok() && !cancelResponseFuture.isDone()) { @@ -246,7 +221,6 @@ public final void testCancelGoal() throws Exception { } } - System.out.println("TEST cancel response received"); assertEquals(true, cancelResponseFuture.isDone()); action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); List goalsCanceling = cancelResponse.getGoalsCanceling(); @@ -256,8 +230,6 @@ public final void testCancelGoal() throws Exception { assertNotEquals(null, this.cancelCallback.goalHandle); test_msgs.action.Fibonacci_Goal cancelingGoal = (test_msgs.action.Fibonacci_Goal)this.cancelCallback.goalHandle.getGoal(); assertEquals(42, cancelingGoal.getOrder()); - - System.out.println("TEST END cancel test"); } /* @Test From 5a50ddcf484c108a4d77ba0f9352ca6a71246623 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 16 Nov 2020 10:45:16 -0800 Subject: [PATCH 13/32] more cleanup Signed-off-by: Jacob Perron --- .../org/ros2/rcljava/action/ActionServer.java | 5 --- .../ros2/rcljava/action/ActionServerImpl.java | 8 ---- .../ros2/rcljava/action/ActionServerTest.java | 42 ------------------- 3 files changed, 55 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java index b3ba2082..eb9da9b5 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java @@ -50,11 +50,6 @@ public interface ActionServer extends Disposable { */ int getNumberOfServices(); - /** - * Get the goal handles that the action server is currently tracking. - */ - // Collection> getGoalHandles(); - /** * Check if an entity of the action server is ready in the wait set. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index b3229208..39f26e4d 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -122,7 +122,6 @@ public synchronized void execute() { if (!this.isCanceling()) { nativeGoalEventExecute(this.handle); } - // self._action_server.notify_execute(self, execute_callback) } /** @@ -304,13 +303,6 @@ public boolean isReady(long waitSetHandle) { return false; } - /** - * {@inheritDoc} - */ - // public Collection> getGoalHandles() { - // return this.goalHandles.values(); - // } - private ActionServerGoalHandle executeGoalRequest( RMWRequestId rmwRequestId, GoalRequestDefinition requestMessage, diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java index b6566643..622859ae 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -231,46 +231,4 @@ public final void testCancelGoal() throws Exception { test_msgs.action.Fibonacci_Goal cancelingGoal = (test_msgs.action.Fibonacci_Goal)this.cancelCallback.goalHandle.getGoal(); assertEquals(42, cancelingGoal.getOrder()); } - /* - @Test - public final void testGetResult() throws Exception { - assertNotEquals(0, this.actionServer.getHandle()); - assertEquals(1, this.node.getActionServers().size()); - - // Send a goal - test_msgs.action.Fibonacci_SendGoal_Response response = sendGoal(42); - - assertNotEquals(null, response); - - // Request the result - test_msgs.srv.GetResult_Request getResultRequest = new test_msgs.srv.GetResult_Request(); - //TODO - // getResultRequest. - action_msgs.msg.GoalInfo goalInfo = new action_msgs.msg.GoalInfo(); - cancelRequest.setGoalInfo(goalInfo); - Future cancelResponseFuture = - this.mockActionClient.cancelGoalClient.asyncSendRequest(cancelRequest); - - // Wait for cancel response - long startTime = System.nanoTime(); - while (RCLJava.ok() && !cancelResponseFuture.isDone()) { - this.executor.spinOnce(0); - - // Check for timeout - long duration = System.nanoTime() - startTime; - if (TimeUnit.NANOSECONDS.toSeconds(duration) >= 5) { - break; - } - } - assertEquals(true, cancelResponseFuture.isDone()); - action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); - List goalsCanceling = cancelResponse.getGoalsCanceling(); - assertEquals(1, goalsCanceling.size()); - - // Assert cancel callback was triggered - assertNotEquals(null, this.cancelCallback.goalHandle); - test_msgs.action.Fibonacci_Goal cancelingGoal = (test_msgs.action.Fibonacci_Goal)this.cancelCallback.goalHandle.getGoal(); - assertEquals(42, cancelingGoal.getOrder()); - } - */ } From af943df368093c92d81e08e29a585c4a0460d10b Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 16 Nov 2020 10:54:44 -0800 Subject: [PATCH 14/32] Minor refactor: move response enums into callback interfaces Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 2 -- .../org/ros2/rcljava/action/ActionServer.java | 1 - .../ros2/rcljava/action/ActionServerImpl.java | 8 +++---- .../ros2/rcljava/action/CancelCallback.java | 5 ++++ .../ros2/rcljava/action/CancelResponse.java | 23 ------------------- .../org/ros2/rcljava/action/GoalCallback.java | 5 ++++ .../org/ros2/rcljava/action/GoalResponse.java | 23 ------------------- 7 files changed, 14 insertions(+), 53 deletions(-) delete mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java delete mode 100644 rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index e2373558..fa1c35eb 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -141,9 +141,7 @@ set(${PROJECT_NAME}_sources "src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java" "src/main/java/org/ros2/rcljava/action/ActionServerImpl.java" "src/main/java/org/ros2/rcljava/action/CancelCallback.java" - "src/main/java/org/ros2/rcljava/action/CancelResponse.java" "src/main/java/org/ros2/rcljava/action/GoalCallback.java" - "src/main/java/org/ros2/rcljava/action/GoalResponse.java" "src/main/java/org/ros2/rcljava/action/GoalStatus.java" "src/main/java/org/ros2/rcljava/client/Client.java" "src/main/java/org/ros2/rcljava/client/ClientImpl.java" diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java index eb9da9b5..97202c8e 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java @@ -21,7 +21,6 @@ import org.ros2.rcljava.interfaces.ActionDefinition; public interface ActionServer extends Disposable { - /** * Get the number of underlying subscriptions that the action server uses. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 39f26e4d..e52e3173 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -334,9 +334,9 @@ private ActionServerGoalHandle executeGoalRequest( // Workaround type GoalCallback callback = ((ActionServerImpl) this).goalCallback; // Call user callback - GoalResponse response = callback.handleGoal(requestMessage); + GoalCallback.GoalResponse response = callback.handleGoal(requestMessage); - boolean accepted = GoalResponse.ACCEPT == response; + boolean accepted = GoalCallback.GoalResponse.ACCEPT == response; responseMessage.accept(accepted); System.out.println("Goal request handled " + accepted); @@ -367,9 +367,9 @@ private action_msgs.srv.CancelGoal_Response executeCancelRequest( continue; } GoalHandleImpl goalHandle = this.goalHandles.get(goalUuid); - CancelResponse cancelResponse = this.cancelCallback.handleCancel(goalHandle); + CancelCallback.CancelResponse cancelResponse = this.cancelCallback.handleCancel(goalHandle); - if (CancelResponse.ACCEPT == cancelResponse) { + if (CancelCallback.CancelResponse.ACCEPT == cancelResponse) { // Update goal state to CANCELING goalHandle.cancelGoal(); diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java index a7203c5e..f16147e6 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/CancelCallback.java @@ -18,6 +18,11 @@ import org.ros2.rcljava.interfaces.ActionDefinition; public interface CancelCallback { + enum CancelResponse { + REJECT, + ACCEPT, + }; + /** * Called when a new cancel request is received. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java b/rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java deleted file mode 100644 index 8695bee8..00000000 --- a/rcljava/src/main/java/org/ros2/rcljava/action/CancelResponse.java +++ /dev/null @@ -1,23 +0,0 @@ -/* Copyright 2020 ros2-java contributors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package org.ros2.rcljava.action; - -// TODO(jacobperron): Consider moving inside Server interface -// ie. Server.CancelResponse.ACCEPT -enum CancelResponse { - REJECT, - ACCEPT, -} diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java index c6e17b4b..5779bd63 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -18,6 +18,11 @@ import org.ros2.rcljava.interfaces.MessageDefinition; public interface GoalCallback { + public enum GoalResponse { + REJECT, + ACCEPT, + }; + /** * Called when a new goal request is received. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java deleted file mode 100644 index 6318f821..00000000 --- a/rcljava/src/main/java/org/ros2/rcljava/action/GoalResponse.java +++ /dev/null @@ -1,23 +0,0 @@ -/* Copyright 2020 ros2-java contributors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package org.ros2.rcljava.action; - -// TODO(jacobperron): Consider moving inside Server interface -// ie. Server.GoalResponse.ACCEPT -enum GoalResponse { - REJECT, - ACCEPT, -} From 7929ece50dbc06845cbff4b1521e8cb1755aab2a Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 16 Nov 2020 11:00:42 -0800 Subject: [PATCH 15/32] Remove TODO The goal request already contains a getter for the goal ID. Signed-off-by: Jacob Perron --- rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java | 1 - 1 file changed, 1 deletion(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java index 5779bd63..85b773cc 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -29,6 +29,5 @@ public enum GoalResponse { * @param goal The action goal request. * @return Goal response indicating if the goal was accepted or not. */ - // TODO(jacobperron): Add UUID parameter GoalResponse handleGoal(T goal); } From 98fd96f67ac5a8b997a8badba792bd42b4bd004a Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 8 Jan 2021 12:03:48 -0800 Subject: [PATCH 16/32] Fix accident Signed-off-by: Jacob Perron --- rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java index 37c30673..ee3fe8d3 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/node/NodeTest.java @@ -1,4 +1,4 @@ -/* Cppyright 2016-2018 Esteve Fernandez +/* Copyright 2016-2018 Esteve Fernandez * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From 1a3403866121c6475e0202fbc7a2d1e91c259a96 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 8 Jan 2021 12:04:36 -0800 Subject: [PATCH 17/32] Alphabetize Signed-off-by: Jacob Perron --- rcljava/CMakeLists.txt | 12 ++++++------ rcljava/package.xml | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt index fa1c35eb..983eabf4 100644 --- a/rcljava/CMakeLists.txt +++ b/rcljava/CMakeLists.txt @@ -110,13 +110,13 @@ foreach(_jni_source ${${PROJECT_NAME}_jni_sources}) endif() ament_target_dependencies(${_target_name} + "action_msgs" + "builtin_interfaces" "rcl" "rcl_action" - "rcljava_common" - "builtin_interfaces" "rcl_interfaces" + "rcljava_common" "rosgraph_msgs" - "action_msgs" "unique_identifier_msgs" ) @@ -219,11 +219,11 @@ add_jar("${PROJECT_NAME}_jar" OUTPUT_NAME ${PROJECT_NAME} INCLUDE_JARS - ${rcljava_common_JARS} + ${action_msgs_JARS} ${builtin_interfaces_JARS} ${rcl_interfaces_JARS} + ${rcljava_common_JARS} ${rosgraph_msgs_JARS} - ${action_msgs_JARS} ${unique_identifier_msgs_JARS} ) @@ -262,10 +262,10 @@ if(BUILD_TESTING) ${${PROJECT_NAME}_message_files} ${${PROJECT_NAME}_service_files} DEPENDENCIES + action_msgs builtin_interfaces rcl_interfaces rosgraph_msgs - action_msgs unique_identifier_msgs ${_java_type_supports} SKIP_INSTALL diff --git a/rcljava/package.xml b/rcljava/package.xml index ffe81e41..d966ac03 100644 --- a/rcljava/package.xml +++ b/rcljava/package.xml @@ -14,9 +14,9 @@ action_msgs builtin_interfaces - rcl_interfaces rcl rcl_action + rcl_interfaces rcpputils rmw_implementation_cmake rmw @@ -34,9 +34,9 @@ action_msgs builtin_interfaces - rcl_interfaces rcl rcl_action + rcl_interfaces rcpputils rmw_implementation_cmake rmw_implementation From 08d180a911c93a2135e225ce27936edd6ed56008 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 8 Jan 2021 12:24:02 -0800 Subject: [PATCH 18/32] Refactor: replace 'getNumberOf*()' JNI functions with single 'getNumberOfEntites()' function Signed-off-by: Jacob Perron --- ...org_ros2_rcljava_action_ActionServerImpl.h | 37 ++------ ...g_ros2_rcljava_action_ActionServerImpl.cpp | 84 ++++++++----------- .../ros2/rcljava/action/ActionServerImpl.java | 13 ++- 3 files changed, 44 insertions(+), 90 deletions(-) diff --git a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h index 53a2832b..28b65441 100644 --- a/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h +++ b/rcljava/include/org_ros2_rcljava_action_ActionServerImpl.h @@ -23,38 +23,13 @@ extern "C" { /* * Class: org_ros2_rcljava_action_ActionServerImpl - * Method: nativeGetNumberOfSubscriptions - * Signature: (L)I + * Method: nativeGetNumberOfEntities + * Signature: (L)[I + * Returns array of numbers for each type of entity, + * [subscriptions, guard_conditions, timers, clients, services] */ -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfSubscriptions( - JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_action_ActionServerImpl - * Method: nativeGetNumberOfTimers - * Signature: (L)I - */ -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfTimers( - JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_action_ActionServerImpl - * Method: nativeGetNumberOfClients - * Signature: (L)I - */ -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfClients( - JNIEnv *, jclass, jlong); - -/* - * Class: org_ros2_rcljava_action_ActionServerImpl - * Method: nativeGetNumberOfServices - * Signature: (L)I - */ -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfServices( +JNIEXPORT jintArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfEntities( JNIEnv *, jclass, jlong); /* diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp index 1f9be797..89799a0b 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -35,59 +35,41 @@ using rcljava_common::signatures::convert_from_java_signature; using rcljava_common::signatures::convert_to_java_signature; using rcljava_common::signatures::destroy_ros_message_signature; -#define RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(Type) \ - do { \ - size_t num_subscriptions; \ - size_t num_guard_conditions; \ - size_t num_timers; \ - size_t num_clients; \ - size_t num_services; \ - rcl_action_server_t * action_server = reinterpret_cast( \ - action_server_handle); \ - rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( \ - action_server, \ - &num_subscriptions, \ - &num_guard_conditions, \ - &num_timers, \ - &num_clients, \ - &num_services); \ - if (ret != RCL_RET_OK) { \ - std::string msg = \ - "Failed to get number of entities for an action server: " + \ - std::string(rcl_get_error_string().str); \ - rcl_reset_error(); \ - rcljava_throw_rclexception(env, ret, msg); \ - } \ - return static_cast(num_ ## Type ## s); \ - } \ - while (0) - -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfSubscriptions( - JNIEnv * env, jclass, jlong action_server_handle) -{ - RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(subscription); -} - -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfTimers( +JNIEXPORT jintArray +JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfEntities( JNIEnv * env, jclass, jlong action_server_handle) { - RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(timer); -} - -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfClients( - JNIEnv * env, jclass, jlong action_server_handle) -{ - RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(client); -} - -JNIEXPORT jint -JNICALL Java_org_ros2_rcljava_action_ActionServerImpl_nativeGetNumberOfServices( - JNIEnv * env, jclass, jlong action_server_handle) -{ - RCLJAVA_ACTION_SERVER_GET_NUMBER_OF_ENTITY(service); + size_t num_subscriptions; + size_t num_guard_conditions; + size_t num_timers; + size_t num_clients; + size_t num_services; + rcl_action_server_t * action_server = reinterpret_cast( + action_server_handle); + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (ret != RCL_RET_OK) { + std::string msg = + "Failed to get number of entities for an action server: " + + std::string(rcl_get_error_string().str); + rcl_reset_error(); + rcljava_throw_rclexception(env, ret, msg); + } + jintArray result = env->NewIntArray(5); + jint temp_result[5] = { + static_cast(num_subscriptions), + static_cast(num_guard_conditions), + static_cast(num_timers), + static_cast(num_clients), + static_cast(num_services) + }; + env->SetIntArrayRegion(result, 0, 5, temp_result); + return result; } JNIEXPORT jbooleanArray diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index e52e3173..b3a05f0c 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -254,37 +254,34 @@ public ActionServerImpl( // node.addWaitable(this); } - private static native int nativeGetNumberOfSubscriptions(long handle); - private static native int nativeGetNumberOfTimers(long handle); - private static native int nativeGetNumberOfClients(long handle); - private static native int nativeGetNumberOfServices(long handle); + private static native int[] nativeGetNumberOfEntities(long handle); /** * {@inheritDoc} */ public int getNumberOfSubscriptions() { - return nativeGetNumberOfSubscriptions(this.handle); + return nativeGetNumberOfEntities(this.handle)[0]; } /** * {@inheritDoc} */ public int getNumberOfTimers() { - return nativeGetNumberOfTimers(this.handle); + return nativeGetNumberOfEntities(this.handle)[2]; } /** * {@inheritDoc} */ public int getNumberOfClients() { - return nativeGetNumberOfClients(this.handle); + return nativeGetNumberOfEntities(this.handle)[3]; } /** * {@inheritDoc} */ public int getNumberOfServices() { - return nativeGetNumberOfServices(this.handle); + return nativeGetNumberOfEntities(this.handle)[4]; } private static native boolean[] nativeGetReadyEntities( From ac784b3d9c9825ffdf30497bbacb58b5ae28d2ad Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 8 Jan 2021 12:36:35 -0800 Subject: [PATCH 19/32] Avoid string copy Signed-off-by: Jacob Perron --- .../cpp/org_ros2_rcljava_action_ActionServerImpl.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp index 89799a0b..caf6997a 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -158,10 +158,7 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( jlong jts = env->CallStaticLongMethod(jaction_class, mid); assert(jts != 0); - const char * action_name_tmp = env->GetStringUTFChars(jaction_name, 0); - - std::string action_name(action_name_tmp); - env->ReleaseStringUTFChars(jaction_name, action_name_tmp); + const char * action_name = env->GetStringUTFChars(jaction_name, 0); rcl_node_t * node = reinterpret_cast(node_handle); rcl_clock_t * clock = reinterpret_cast(clock_handle); @@ -174,8 +171,8 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); rcl_ret_t ret = rcl_action_server_init( - action_server, node, clock, ts, action_name.c_str(), &action_server_ops); - // env->ReleaseStringUTFChars(jaction_name, action_name); + action_server, node, clock, ts, action_name, &action_server_ops); + env->ReleaseStringUTFChars(jaction_name, action_name); if (ret != RCL_RET_OK) { std::string msg = "Failed to create action server: " + std::string(rcl_get_error_string().str); From d61317534816c92a051a97a9638bfc2d30fc9550 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 8 Jan 2021 13:19:22 -0800 Subject: [PATCH 20/32] Remove extern C Replace with 'namespace rcljava' Signed-off-by: Jacob Perron --- rcljava/src/main/cpp/convert.hpp | 11 +++++------ .../cpp/org_ros2_rcljava_action_ActionServerImpl.cpp | 4 ++-- .../cpp/org_ros2_rcljava_executors_BaseExecutor.cpp | 6 +++--- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/rcljava/src/main/cpp/convert.hpp b/rcljava/src/main/cpp/convert.hpp index 72b64c30..a07e4a49 100644 --- a/rcljava/src/main/cpp/convert.hpp +++ b/rcljava/src/main/cpp/convert.hpp @@ -21,9 +21,9 @@ #ifndef MAIN__CPP__CONVERT_HPP_ #define MAIN__CPP__CONVERT_HPP_ -#ifdef __cplusplus -extern "C" { -#endif + +namespace rcljava +{ jobject convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) @@ -83,7 +83,6 @@ convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) return request_id; } -#ifdef __cplusplus -} -#endif +} // namespace rcljava + #endif // MAIN__CPP__CONVERT_HPP_ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp index caf6997a..2d48fef1 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_action_ActionServerImpl.cpp @@ -213,7 +213,7 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( jobject jtaken_msg = convert_to_java(taken_msg, jrequest_msg); \ destroy_ros_message(taken_msg); \ assert(jtaken_msg != nullptr); \ - jobject jheader = convert_rmw_request_id_to_java(env, &header); \ + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); \ return jheader; \ } \ destroy_ros_message(taken_msg); \ @@ -231,7 +231,7 @@ Java_org_ros2_rcljava_action_ActionServerImpl_nativeCreateActionServer( convert_from_java_signature convert_from_java = \ reinterpret_cast(jresponse_from_java_converter_handle); \ void * response_msg = convert_from_java(jresponse_msg, nullptr); \ - rmw_request_id_t * request_id = convert_rmw_request_id_from_java(env, jrequest_id); \ + rmw_request_id_t * request_id = rcljava::convert_rmw_request_id_from_java(env, jrequest_id); \ rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ action_server, request_id, response_msg); \ destroy_ros_message_signature destroy_ros_message = \ diff --git a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp index 8682e5b1..2a784dab 100644 --- a/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp +++ b/rcljava/src/main/cpp/org_ros2_rcljava_executors_BaseExecutor.cpp @@ -297,7 +297,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeRequest( assert(jtaken_msg != nullptr); - jobject jheader = convert_rmw_request_id_to_java(env, &header); + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } @@ -325,7 +325,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeSendServiceResponse( void * response_msg = convert_from_java(jresponse_msg, nullptr); - rmw_request_id_t * request_id = convert_rmw_request_id_from_java(env, jrequest_id); + rmw_request_id_t * request_id = rcljava::convert_rmw_request_id_from_java(env, jrequest_id); rcl_ret_t ret = rcl_send_response(service, request_id, response_msg); @@ -387,7 +387,7 @@ Java_org_ros2_rcljava_executors_BaseExecutor_nativeTakeResponse( assert(jtaken_msg != nullptr); - jobject jheader = convert_rmw_request_id_to_java(env, &header); + jobject jheader = rcljava::convert_rmw_request_id_to_java(env, &header); return jheader; } From ed684b12201ed579259d8356cad93e6e25634a60 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 8 Jan 2021 14:41:12 -0800 Subject: [PATCH 21/32] Remove unnecessary synchronization Signed-off-by: Jacob Perron --- .../main/java/org/ros2/rcljava/action/ActionServerImpl.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index b3a05f0c..646f846f 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -86,14 +86,14 @@ public GoalHandleImpl( /** * {@inheritDoc} */ - public synchronized action_msgs.msg.GoalInfo getGoalInfo() { + public action_msgs.msg.GoalInfo getGoalInfo() { return this.goalInfo; } /** * {@inheritDoc} */ - public synchronized MessageDefinition getGoal() { + public MessageDefinition getGoal() { return this.goal; } @@ -164,7 +164,7 @@ public synchronized final void dispose() { * {@inheritDoc} */ public synchronized final long getHandle() { - return handle; + return this.handle; } } // class GoalHandleImpl From f2de7627cebd7c19447be5c2f2d03afbb4db2c8b Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 8 Jan 2021 15:23:44 -0800 Subject: [PATCH 22/32] Refactor access to goalCallback Signed-off-by: Jacob Perron --- .../main/java/org/ros2/rcljava/action/ActionServerImpl.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 646f846f..2587bcd6 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -328,9 +328,8 @@ private ActionServerGoalHandle executeGoalRequest( return null; } - // Workaround type - GoalCallback callback = ((ActionServerImpl) this).goalCallback; // Call user callback + GoalCallback callback = this.goalCallback; GoalCallback.GoalResponse response = callback.handleGoal(requestMessage); boolean accepted = GoalCallback.GoalResponse.ACCEPT == response; From 11b107e2cd0d5258efeba326da82789544da514b Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Jan 2021 11:20:22 -0800 Subject: [PATCH 23/32] typesafe request and response definitions Signed-off-by: Jacob Perron --- .../org/ros2/rcljava/action/ActionServerImpl.java | 12 ++++++------ .../src/main/java/org/ros2/rcljava/node/Node.java | 2 +- .../main/java/org/ros2/rcljava/node/NodeImpl.java | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 2587bcd6..dc478764 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -184,7 +184,7 @@ public synchronized final long getHandle() { private final T actionTypeInstance; private final String actionName; private long handle; - private final GoalCallback goalCallback; + private final GoalCallback> goalCallback; private final CancelCallback cancelCallback; private final Consumer> acceptedCallback; @@ -225,7 +225,7 @@ public ActionServerImpl( final WeakReference nodeReference, final Class actionType, final String actionName, - final GoalCallback goalCallback, + final GoalCallback> goalCallback, final CancelCallback cancelCallback, final Consumer> acceptedCallback) throws IllegalArgumentException { this.nodeReference = nodeReference; @@ -302,8 +302,8 @@ public boolean isReady(long waitSetHandle) { private ActionServerGoalHandle executeGoalRequest( RMWRequestId rmwRequestId, - GoalRequestDefinition requestMessage, - GoalResponseDefinition responseMessage) + GoalRequestDefinition requestMessage, + GoalResponseDefinition responseMessage) { builtin_interfaces.msg.Time timeRequestHandled = this.clock.now().toMsg(); responseMessage.setStamp(timeRequestHandled.getSec(), timeRequestHandled.getNanosec()); @@ -445,8 +445,8 @@ public void execute() { Class requestType = this.actionTypeInstance.getSendGoalRequestType(); Class responseType = this.actionTypeInstance.getSendGoalResponseType(); - GoalRequestDefinition requestMessage = null; - GoalResponseDefinition responseMessage = null; + GoalRequestDefinition requestMessage = null; + GoalResponseDefinition responseMessage = null; try { requestMessage = requestType.newInstance(); diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index a486c121..632176eb 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -146,7 +146,7 @@ Client createClient(final Class serviceType, ActionServer createActionServer(final Class actionType, final String actionName, - final GoalCallback goalCallback, + final GoalCallback> goalCallback, final CancelCallback cancelCallback, final Consumer> acceptedCallback); diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java index 914b369e..7c6dd4b4 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/NodeImpl.java @@ -403,7 +403,7 @@ private static native long nativeCreateClientHandl public ActionServer createActionServer(final Class actionType, final String actionName, - final GoalCallback goalCallback, + final GoalCallback> goalCallback, final CancelCallback cancelCallback, final Consumer> acceptedCallback) throws IllegalArgumentException { ActionServer actionServer = new ActionServerImpl( From 6203cead13af7973abbfdab409d6a6c25c809058 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Jan 2021 12:10:46 -0800 Subject: [PATCH 24/32] Be more specific about template type for GoalCallback Signed-off-by: Jacob Perron --- .../src/main/java/org/ros2/rcljava/action/GoalCallback.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java index 85b773cc..afd017b0 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -15,9 +15,9 @@ package org.ros2.rcljava.action; -import org.ros2.rcljava.interfaces.MessageDefinition; +import org.ros2.rcljava.interfaces.GoalRequestDefinition; -public interface GoalCallback { +public interface GoalCallback { public enum GoalResponse { REJECT, ACCEPT, From 5e4693ad1472d96b3c1eed4de4d31ba35b11dc3d Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Jan 2021 14:22:03 -0800 Subject: [PATCH 25/32] Minor refactor Signed-off-by: Jacob Perron --- .../main/java/org/ros2/rcljava/action/ActionServerImpl.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index dc478764..5543a0be 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -184,7 +184,7 @@ public synchronized final long getHandle() { private final T actionTypeInstance; private final String actionName; private long handle; - private final GoalCallback> goalCallback; + private final GoalCallback goalCallback; private final CancelCallback cancelCallback; private final Consumer> acceptedCallback; @@ -329,8 +329,7 @@ private ActionServerGoalHandle executeGoalRequest( } // Call user callback - GoalCallback callback = this.goalCallback; - GoalCallback.GoalResponse response = callback.handleGoal(requestMessage); + GoalCallback.GoalResponse response = this.goalCallback.handleGoal(requestMessage); boolean accepted = GoalCallback.GoalResponse.ACCEPT == response; responseMessage.accept(accepted); From 9c93a03be4f5ff0a1ac024cfd05645f788b243c3 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Jan 2021 15:42:48 -0800 Subject: [PATCH 26/32] Make GoalHandleImpl inner class instead of static Signed-off-by: Jacob Perron --- .../ros2/rcljava/action/ActionServerImpl.java | 53 ++++++++----------- 1 file changed, 22 insertions(+), 31 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 5543a0be..b3628da1 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -38,35 +38,37 @@ import org.slf4j.LoggerFactory; public class ActionServerImpl implements ActionServer { - static class GoalHandleImpl implements ActionServerGoalHandle { - private static final Logger logger = LoggerFactory.getLogger(GoalHandleImpl.class); + private static final Logger logger = LoggerFactory.getLogger(ActionServerImpl.class); - static { - try { - JNIUtils.loadImplementation(GoalHandleImpl.class); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } + static { + try { + JNIUtils.loadImplementation(ActionServerImpl.class); + JNIUtils.loadImplementation(ActionServerImpl.GoalHandleImpl.class); + } catch (UnsatisfiedLinkError ule) { + logger.error("Native code library failed to load.\n" + ule); + System.exit(1); } + JNIUtils.loadImplementation(ActionServerImpl.class); + } + class GoalHandleImpl implements ActionServerGoalHandle { private long handle; private ActionServer actionServer; private action_msgs.msg.GoalInfo goalInfo; private MessageDefinition goal; - private static native long nativeAcceptNewGoal( + private native long nativeAcceptNewGoal( long actionServerHandle, long goalInfoFromJavaConverterHandle, long goalInfoDestructorHandle, MessageDefinition goalInfo); - private static native int nativeGetStatus(long goalHandle); - private static native void nativeGoalEventExecute(long goalHandle); - private static native void nativeGoalEventCancelGoal(long goalHandle); - private static native void nativeGoalEventSucceed(long goalHandle); - private static native void nativeGoalEventAbort(long goalHandle); - private static native void nativeGoalEventCanceled(long goalHandle); - private static native void nativeDispose(long handle); + private native int nativeGetStatus(long goalHandle); + private native void nativeGoalEventExecute(long goalHandle); + private native void nativeGoalEventCancelGoal(long goalHandle); + private native void nativeGoalEventSucceed(long goalHandle); + private native void nativeGoalEventAbort(long goalHandle); + private native void nativeGoalEventCanceled(long goalHandle); + private native void nativeDispose(long handle); public GoalHandleImpl( ActionServer actionServer, action_msgs.msg.GoalInfo goalInfo, MessageDefinition goal) @@ -168,17 +170,6 @@ public synchronized final long getHandle() { } } // class GoalHandleImpl - private static final Logger logger = LoggerFactory.getLogger(ActionServerImpl.class); - - static { - try { - JNIUtils.loadImplementation(ActionServerImpl.class); - } catch (UnsatisfiedLinkError ule) { - logger.error("Native code library failed to load.\n" + ule); - System.exit(1); - } - } - private final WeakReference nodeReference; private final Clock clock; private final T actionTypeInstance; @@ -190,7 +181,7 @@ public synchronized final long getHandle() { private boolean[] readyEntities; - private Map, GoalHandleImpl> goalHandles; + private Map, GoalHandleImpl> goalHandles; private boolean isGoalRequestReady() { return this.readyEntities[0]; @@ -239,7 +230,7 @@ public ActionServerImpl( this.cancelCallback = cancelCallback; this.acceptedCallback = acceptedCallback; - this.goalHandles = new HashMap, GoalHandleImpl>(); + this.goalHandles = new HashMap, GoalHandleImpl>(); Node node = nodeReference.get(); if (node == null) { @@ -340,7 +331,7 @@ private ActionServerGoalHandle executeGoalRequest( } // Create a goal handle and add it to the list of goals - GoalHandleImpl goalHandle = new GoalHandleImpl( + GoalHandleImpl goalHandle = this.new GoalHandleImpl( this, goalInfo, requestMessage.getGoal()); this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); return goalHandle; From f6dbc953cdb793ad4a9732aa772b6fb893503c0c Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Jan 2021 17:10:22 -0800 Subject: [PATCH 27/32] Remove synchronized from getHandle Signed-off-by: Jacob Perron --- .../src/main/java/org/ros2/rcljava/action/ActionServerImpl.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index b3628da1..a844dbcb 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -165,7 +165,7 @@ public synchronized final void dispose() { /** * {@inheritDoc} */ - public synchronized final long getHandle() { + public final long getHandle() { return this.handle; } } // class GoalHandleImpl From c652771557d7fcf3809f64b243898af79f89d51c Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 13 Jan 2021 17:13:46 -0800 Subject: [PATCH 28/32] Add TODO for Waitable interface Signed-off-by: Jacob Perron --- rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java index 97202c8e..4ab12f2e 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServer.java @@ -21,6 +21,7 @@ import org.ros2.rcljava.interfaces.ActionDefinition; public interface ActionServer extends Disposable { + // TODO(jacobperron): Move most of these methods to a new "Waitable" interface /** * Get the number of underlying subscriptions that the action server uses. * From 59b2d2464ea4b1cba309599a6410cde808cf1920 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Jan 2021 11:10:59 -0800 Subject: [PATCH 29/32] Fix style Signed-off-by: Jacob Perron --- .../java/org/ros2/rcljava/executors/BaseExecutor.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java index f180cf56..9ad11e52 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java +++ b/rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java @@ -240,9 +240,9 @@ protected void waitForWork(long timeout) { for (ActionServer actionServer : node.getNode().getActionServers()) { subscriptionsSize += actionServer.getNumberOfSubscriptions(); - timersSize += actionServer.getNumberOfTimers(); - clientsSize += actionServer.getNumberOfClients(); - servicesSize += actionServer.getNumberOfServices(); + timersSize += actionServer.getNumberOfTimers(); + clientsSize += actionServer.getNumberOfClients(); + servicesSize += actionServer.getNumberOfServices(); } } @@ -421,8 +421,8 @@ protected AnyExecutable getNextExecutable() { for (Map.Entry entry : this.actionServerHandles) { if (entry.getValue() != null) { anyExecutable.actionServer = entry.getValue(); - entry.setValue(null); - return anyExecutable; + entry.setValue(null); + return anyExecutable; } } From b4b79229437ebb5e20e74fb3cfe9df1612435565 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Jan 2021 11:11:10 -0800 Subject: [PATCH 30/32] Minor refactor Signed-off-by: Jacob Perron --- rcljava/src/main/cpp/convert.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcljava/src/main/cpp/convert.hpp b/rcljava/src/main/cpp/convert.hpp index a07e4a49..31db9655 100644 --- a/rcljava/src/main/cpp/convert.hpp +++ b/rcljava/src/main/cpp/convert.hpp @@ -47,7 +47,7 @@ convert_rmw_request_id_to_java(JNIEnv * env, rmw_request_id_t * request_id) env->SetLongField(jrequest_id, jsequence_number_field_id, sequence_number); - jsize writer_guid_len = 16; // See rmw/rmw/include/rmw/types.h + jsize writer_guid_len = sizeof(request_id->writer_guid) / sizeof(request_id->writer_guid[0]); jbyteArray jwriter_guid = env->NewByteArray(writer_guid_len); env->SetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); @@ -75,7 +75,7 @@ convert_rmw_request_id_from_java(JNIEnv * env, jobject jrequest_id) 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 + jsize writer_guid_len = sizeof(request_id->writer_guid) / sizeof(request_id->writer_guid[0]); jbyteArray jwriter_guid = (jbyteArray)env->GetObjectField(jrequest_id, jwriter_guid_field_id); env->GetByteArrayRegion(jwriter_guid, 0, writer_guid_len, reinterpret_cast(writer_guid)); From 6014fad407d9f11ab7c6eada9c1e142da3cfa26d Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Jan 2021 11:16:17 -0800 Subject: [PATCH 31/32] Add docs for createActionServer Signed-off-by: Jacob Perron --- .../src/main/java/org/ros2/rcljava/node/Node.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index 632176eb..63807a81 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -144,6 +144,18 @@ Client createClient( Client createClient(final Class serviceType, final String serviceName) throws NoSuchFieldException, IllegalAccessException; + /** + * Create an ActionServer<T>. + * + * @param The type of action that will be handled by the created @{link ActionServer}. + * @param actionName The name of action that the create @{link ActionServer} will offer. + * @param goalCallback The callback that will be called when the @{link ActionServer} + * receives a new goal request. + * @param cancelCallback The callback that will be called when the @{link ActionServer} + * receives a cancle request for an active goal. + * @param acceptedCallback The callback that will be called when the @{link ActionServer} + * accepts a goal request. + */ ActionServer createActionServer(final Class actionType, final String actionName, final GoalCallback> goalCallback, From f94150b1a8a9bbf87e299471f3c632acca4e9b6e Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Jan 2021 11:36:53 -0800 Subject: [PATCH 32/32] Switch from List to array Signed-off-by: Jacob Perron --- .../main/java/org/ros2/rcljava/action/ActionServerImpl.java | 2 +- .../test/java/org/ros2/rcljava/action/ActionServerTest.java | 4 ++-- .../org/ros2/rcljava/interfaces/GoalRequestDefinition.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index a844dbcb..8c883f25 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -346,7 +346,7 @@ private action_msgs.srv.CancelGoal_Response executeCancelRequest( // Process user callback for each goal in cancel request for (action_msgs.msg.GoalInfo goalInfo : inputMessage.getGoalsCanceling()) { - List goalUuid = goalInfo.getGoalId().getUuid(); + List goalUuid = goalInfo.getGoalId().getUuidAsList(); // It's possible a goal may not be tracked by the user if (!this.goalHandles.containsKey(goalUuid)) { logger.warn("Ignoring cancel request for untracked goal handle with ID '" + goalUuid + "'"); diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java index 622859ae..36f004b9 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -223,8 +223,8 @@ public final void testCancelGoal() throws Exception { assertEquals(true, cancelResponseFuture.isDone()); action_msgs.srv.CancelGoal_Response cancelResponse = cancelResponseFuture.get(); - List goalsCanceling = cancelResponse.getGoalsCanceling(); - assertEquals(1, goalsCanceling.size()); + action_msgs.msg.GoalInfo[] goalsCanceling = cancelResponse.getGoalsCanceling(); + assertEquals(1, goalsCanceling.length); // Assert cancel callback was triggered assertNotEquals(null, this.cancelCallback.goalHandle); diff --git a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java index af7b9a84..cf6dc177 100644 --- a/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java +++ b/rcljava_common/src/main/java/org/ros2/rcljava/interfaces/GoalRequestDefinition.java @@ -19,5 +19,5 @@ public interface GoalRequestDefinition extends MessageDefinition { MessageDefinition getGoal(); - byte[] getGoalUuid(); + List getGoalUuid(); }