From 5c457ff8d6847e5d4041175b8180af78920ca0ba Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Wed, 29 Mar 2023 21:23:28 +0000 Subject: [PATCH 01/14] Move node impl struct to separate header Signed-off-by: Hans-Joachim Krauch --- rcl/src/rcl/node.c | 11 +---------- rcl/src/rcl/node_impl.h | 31 +++++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+), 10 deletions(-) create mode 100644 rcl/src/rcl/node_impl.h diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index fed56a746..de74a0f82 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -52,19 +52,10 @@ extern "C" #include "tracetools/tracetools.h" #include "./context_impl.h" +#include "./node_impl.h" const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR = "ROS_DISABLE_LOANED_MESSAGES"; -struct rcl_node_impl_s -{ - rcl_node_options_t options; - rmw_node_t * rmw_node_handle; - rcl_guard_condition_t * graph_guard_condition; - const char * logger_name; - const char * fq_name; -}; - - /// Return the logger name associated with a node given the validated node name and namespace. /** * E.g. for a node named "c" in namespace "/a/b", the logger name will be diff --git a/rcl/src/rcl/node_impl.h b/rcl/src/rcl/node_impl.h new file mode 100644 index 000000000..19c089374 --- /dev/null +++ b/rcl/src/rcl/node_impl.h @@ -0,0 +1,31 @@ +// Copyright 2023 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. + +#ifndef RCL__NODE_IMPL_H_ +#define RCL__NODE_IMPL_H_ + +#include "rcl/guard_condition.h" +#include "rcl/node_options.h" +#include "rmw/types.h" + +struct rcl_node_impl_s +{ + rcl_node_options_t options; + rmw_node_t * rmw_node_handle; + rcl_guard_condition_t * graph_guard_condition; + const char * logger_name; + const char * fq_name; +}; + +#endif // RCL__NODE_IMPL_H_ From 9ccf7abbb2f40ec30e1fd2ca99ff6e92108814a7 Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Wed, 29 Mar 2023 21:19:42 +0000 Subject: [PATCH 02/14] Add functions to convert between rosidl_runtime_c / type_description_interfaces structs Signed-off-by: Hans-Joachim Krauch --- rcl/CMakeLists.txt | 1 + .../rcl/type_description_conversions.h | 133 ++++++++ rcl/src/rcl/type_description_conversions.c | 307 ++++++++++++++++++ rcl/test/CMakeLists.txt | 8 + .../rcl/test_type_description_conversions.cpp | 82 +++++ 5 files changed, 531 insertions(+) create mode 100644 rcl/include/rcl/type_description_conversions.h create mode 100644 rcl/src/rcl/type_description_conversions.c create mode 100644 rcl/test/rcl/test_type_description_conversions.cpp diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index eef888492..e296aac4f 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -70,6 +70,7 @@ set(${PROJECT_NAME}_sources src/rcl/time.c src/rcl/timer.c src/rcl/type_hash.c + src/rcl/type_description_conversions.c src/rcl/validate_enclave_name.c src/rcl/validate_topic_name.c src/rcl/wait.c diff --git a/rcl/include/rcl/type_description_conversions.h b/rcl/include/rcl/type_description_conversions.h new file mode 100644 index 000000000..25b604767 --- /dev/null +++ b/rcl/include/rcl/type_description_conversions.h @@ -0,0 +1,133 @@ +// Copyright 2023 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. + +#ifndef RCL__TYPE_DESCRIPTION_CONVERSIONS_H_ +#define RCL__TYPE_DESCRIPTION_CONVERSIONS_H_ + +#include "rcl/macros.h" +#include "rcl/visibility_control.h" +#include "rosidl_runtime_c/type_description/type_description__struct.h" +#include "rosidl_runtime_c/type_description/type_source__struct.h" +#include "type_description_interfaces/msg/type_description.h" +#include "type_description_interfaces/msg/type_source.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/// Convert type description runtime struct to msg struct. +/** + * This function converts a rosidl_runtime_c__type_description__TypeDescription + * to the corresponding type_description_interfaces/msg/TypeDescription struct. + * The retrieved pointer shall be destroyed with + * `type_description_interfaces__msg__TypeDescription__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] runtime_description the pointer to the runtime type description struct + * \return a valid type_description_interfaces/msg/TypeDescription pointer, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +type_description_interfaces__msg__TypeDescription * +rcl_convert_type_description_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeDescription * runtime_description); + +/// Convert type description msg struct to a rosidl runtime struct. +/** + * This function converts a type_description_interfaces/msg/TypeDescription + * to the corresponding rosidl_runtime_c__type_description__TypeDescription + * struct. The retrieved pointer shall be destroyed with + * `rosidl_runtime_c__type_description__TypeDescription__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] description_msg the pointer to the msg type description struct + * \return a valid pointer to the runtime struct, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rosidl_runtime_c__type_description__TypeDescription * +rcl_convert_type_description_msg_to_runtime( + const type_description_interfaces__msg__TypeDescription * description_msg); + +/// Convert type sources sequence runtime struct to msg struct. +/** + * This function converts a rosidl_runtime_c__type_description__TypeSource__Sequence + * struct to the corresponding type_description_interfaces__msg__TypeSource__Sequence + * struct. The retrieved pointer shall be destroyed with + * `type_description_interfaces__msg__TypeSource__Sequence__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] runtime_type_sources the pointer to the type sources sequence struct + * \return a valid pointer to the msg struct, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +type_description_interfaces__msg__TypeSource__Sequence * +rcl_convert_type_source_sequence_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeSource__Sequence * runtime_type_sources); + +/// Convert type sources sequece msg struct to a rosidl runtime struct. +/** + * This function converts a rosidl_runtime_c__type_description__TypeSource__Sequence + * struct to the corresponding type_description_interfaces__msg__TypeSource__Sequence + * struct. The retrieved pointer shall be destroyed with + * `type_description_interfaces__msg__TypeSource__Sequence__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] type_sources_msg the pointer to the type sources sequence struct + * \return a valid pointer to the msg struct, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rosidl_runtime_c__type_description__TypeSource__Sequence * +rcl_convert_type_source_sequence_msg_to_runtime( + const type_description_interfaces__msg__TypeSource__Sequence * type_sources_msg); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__TYPE_DESCRIPTION_CONVERSIONS_H_ diff --git a/rcl/src/rcl/type_description_conversions.c b/rcl/src/rcl/type_description_conversions.c new file mode 100644 index 000000000..977269168 --- /dev/null +++ b/rcl/src/rcl/type_description_conversions.c @@ -0,0 +1,307 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcl/error_handling.h" +#include "rcl/type_description_conversions.h" + +#include "rosidl_runtime_c/string_functions.h" +#include "rosidl_runtime_c/type_description/field__functions.h" +#include "rosidl_runtime_c/type_description/individual_type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_source__functions.h" +#include "type_description_interfaces/msg/detail/field__functions.h" +#include "type_description_interfaces/msg/individual_type_description.h" + +static bool individual_type_description_runtime_to_msg( + const rosidl_runtime_c__type_description__IndividualTypeDescription * in, + type_description_interfaces__msg__IndividualTypeDescription * out) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(in, false); + RCL_CHECK_ARGUMENT_FOR_NULL(out, false); + + const bool success = + rosidl_runtime_c__String__copy(&in->type_name, &out->type_name) && + type_description_interfaces__msg__Field__Sequence__init( + &out->fields, + in->fields.size); + if (!success) { + goto error; + } + + for (size_t i = 0; i < in->fields.size; ++i) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].name), + &(out->fields.data[i].name))) + { + goto error; + } + + if (in->fields.data[i].default_value.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].default_value), + &(out->fields.data[i].default_value))) + { + goto error; + } + } + + // type_id + out->fields.data[i].type.type_id = in->fields.data[i].type.type_id; + // capacity + out->fields.data[i].type.capacity = in->fields.data[i].type.capacity; + // string_capacity + out->fields.data[i].type.string_capacity = + in->fields.data[i].type.string_capacity; + + // nested_type_name + if (in->fields.data[i].type.nested_type_name.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].type.nested_type_name), + &(out->fields.data[i].type.nested_type_name))) + { + goto error; + } + } + } + + return true; + +error: + type_description_interfaces__msg__IndividualTypeDescription__fini(out); + return false; +} + +static bool individual_type_description_msg_to_runtime( + const type_description_interfaces__msg__IndividualTypeDescription * in, + rosidl_runtime_c__type_description__IndividualTypeDescription * out) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(in, false); + RCL_CHECK_ARGUMENT_FOR_NULL(out, false); + + const bool success = + rosidl_runtime_c__String__copy(&in->type_name, &out->type_name) && + rosidl_runtime_c__type_description__Field__Sequence__init( + &out->fields, in->fields.size); + if (!success) { + goto error; + } + + for (size_t i = 0; i < in->fields.size; ++i) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].name), + &(out->fields.data[i].name))) + { + goto error; + } + + if (in->fields.data[i].default_value.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].default_value), + &(out->fields.data[i].default_value))) + { + goto error; + } + } + + // type_id + out->fields.data[i].type.type_id = in->fields.data[i].type.type_id; + // capacity + out->fields.data[i].type.capacity = in->fields.data[i].type.capacity; + // string_capacity + out->fields.data[i].type.string_capacity = + in->fields.data[i].type.string_capacity; + + // nested_type_name + if (in->fields.data[i].type.nested_type_name.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].type.nested_type_name), + &(out->fields.data[i].type.nested_type_name))) + { + goto error; + } + } + } + + return true; + +error: + rosidl_runtime_c__type_description__IndividualTypeDescription__init(out); + return false; +} + +type_description_interfaces__msg__TypeDescription * +rcl_convert_type_description_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeDescription * runtime_description) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(runtime_description, NULL); + + // Create the object + type_description_interfaces__msg__TypeDescription * out = + type_description_interfaces__msg__TypeDescription__create(); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // init referenced_type_descriptions with the correct size + if (!type_description_interfaces__msg__IndividualTypeDescription__Sequence__init( + &out->referenced_type_descriptions, + runtime_description->referenced_type_descriptions.size)) + { + goto fail; + } + + // Convert individual type description + if (!individual_type_description_runtime_to_msg( + &runtime_description->type_description, + &out->type_description)) + { + goto fail; + } + + // Convert referenced type descriptions + for (size_t i = 0; i < runtime_description->referenced_type_descriptions.size; ++i) { + if (!individual_type_description_runtime_to_msg( + &runtime_description->referenced_type_descriptions.data[i], + &out->referenced_type_descriptions.data[i])) + { + goto fail; + } + } + + return out; + +fail: + type_description_interfaces__msg__TypeDescription__destroy(out); + return NULL; +} + +rosidl_runtime_c__type_description__TypeDescription * +rcl_convert_type_description_msg_to_runtime( + const type_description_interfaces__msg__TypeDescription * description_msg) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(description_msg, NULL); + + // Create the object + rosidl_runtime_c__type_description__TypeDescription * out = + rosidl_runtime_c__type_description__TypeDescription__create(); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // init referenced_type_descriptions with the correct size + if (!rosidl_runtime_c__type_description__IndividualTypeDescription__Sequence__init( + &out->referenced_type_descriptions, + description_msg->referenced_type_descriptions.size)) + { + goto fail; + } + + if (!individual_type_description_msg_to_runtime( + &description_msg->type_description, + &out->type_description)) + { + goto fail; + } + + for (size_t i = 0; i < description_msg->referenced_type_descriptions.size; ++i) { + if (!individual_type_description_msg_to_runtime( + &description_msg->referenced_type_descriptions.data[i], + &out->referenced_type_descriptions.data[i])) + { + goto fail; + } + } + + return out; + +fail: + rosidl_runtime_c__type_description__TypeDescription__destroy(out); + return NULL; +} + +type_description_interfaces__msg__TypeSource__Sequence * +rcl_convert_type_source_sequence_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeSource__Sequence * runtime_type_sources) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(runtime_type_sources, NULL); + + // Create the object + type_description_interfaces__msg__TypeSource__Sequence * out = + type_description_interfaces__msg__TypeSource__Sequence__create(runtime_type_sources->size); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // Copy type sources + for (size_t i = 0; i < runtime_type_sources->size; ++i) { + // type_name + if (!rosidl_runtime_c__String__copy( + &(runtime_type_sources->data[i].type_name), &(out->data[i].type_name))) + { + goto fail; + } + // encoding + if (!rosidl_runtime_c__String__copy( + &(runtime_type_sources->data[i].encoding), &(out->data[i].encoding))) + { + goto fail; + } + // raw_file_contents + if (!rosidl_runtime_c__String__copy( + &(runtime_type_sources->data[i].raw_file_contents), &(out->data[i].raw_file_contents))) + { + goto fail; + } + } + + return out; + +fail: + type_description_interfaces__msg__TypeSource__Sequence__destroy(out); + return NULL; +} + +rosidl_runtime_c__type_description__TypeSource__Sequence * +rcl_convert_type_source_sequence_msg_to_runtime( + const type_description_interfaces__msg__TypeSource__Sequence * type_sources_msg) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(type_sources_msg, NULL); + + // Create the object + rosidl_runtime_c__type_description__TypeSource__Sequence * out = + rosidl_runtime_c__type_description__TypeSource__Sequence__create(type_sources_msg->size); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // Copy type sources + for (size_t i = 0; i < type_sources_msg->size; ++i) { + // type_name + if (!rosidl_runtime_c__String__copy( + &(type_sources_msg->data[i].type_name), &(out->data[i].type_name))) + { + goto fail; + } + // encoding + if (!rosidl_runtime_c__String__copy( + &(type_sources_msg->data[i].encoding), &(out->data[i].encoding))) + { + goto fail; + } + // raw_file_contents + if (!rosidl_runtime_c__String__copy( + &(type_sources_msg->data[i].raw_file_contents), &(out->data[i].raw_file_contents))) + { + goto fail; + } + } + + return out; + +fail: + rosidl_runtime_c__type_description__TypeSource__Sequence__destroy(out); + return NULL; +} diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index d106c0ae5..3b9cf43cd 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -328,6 +328,14 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + rcl_add_custom_gtest(test_type_description_conversions${target_suffix} + SRCS rcl/test_type_description_conversions.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES "test_msgs" + ) + # Launch tests rcl_add_custom_executable(service_fixture${target_suffix} diff --git a/rcl/test/rcl/test_type_description_conversions.cpp b/rcl/test/rcl/test_type_description_conversions.cpp new file mode 100644 index 000000000..f0be61e06 --- /dev/null +++ b/rcl/test/rcl/test_type_description_conversions.cpp @@ -0,0 +1,82 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/type_description_conversions.h" +#include "rosidl_runtime_c/message_type_support_struct.h" +#include "rosidl_runtime_c/type_description/type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_source__functions.h" +#include "test_msgs/msg/basic_types.h" + +TEST(TestTypeDescriptionConversions, type_description_conversion_round_trip) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + + type_description_interfaces__msg__TypeDescription * type_description_msg = + rcl_convert_type_description_runtime_to_msg(ts->get_type_description_func(ts)); + EXPECT_TRUE(NULL != type_description_msg); + + rosidl_runtime_c__type_description__TypeDescription * type_description_rt = + rcl_convert_type_description_msg_to_runtime(type_description_msg); + EXPECT_TRUE(NULL != type_description_rt); + + EXPECT_TRUE( + rosidl_runtime_c__type_description__TypeDescription__are_equal( + type_description_rt, ts->get_type_description_func(ts))); + + type_description_interfaces__msg__TypeDescription__destroy( + type_description_msg); + rosidl_runtime_c__type_description__TypeDescription__destroy( + type_description_rt); +} + +TEST(TestTypeDescriptionConversions, type_description_invalid_input) { + EXPECT_TRUE(NULL == rcl_convert_type_description_runtime_to_msg(NULL)); + EXPECT_TRUE(NULL == rcl_convert_type_description_msg_to_runtime(NULL)); +} + +TEST(TestTypeDescriptionConversions, type_source_sequence_conversion_round_trip) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + + type_description_interfaces__msg__TypeSource__Sequence * type_sources_msg = + rcl_convert_type_source_sequence_runtime_to_msg(ts->get_type_description_sources_func(ts)); + EXPECT_TRUE(NULL != type_sources_msg); + + rosidl_runtime_c__type_description__TypeSource__Sequence * type_sources_rt = + rcl_convert_type_source_sequence_msg_to_runtime(type_sources_msg); + EXPECT_TRUE(NULL != type_sources_rt); + + EXPECT_TRUE( + rosidl_runtime_c__type_description__TypeSource__Sequence__are_equal( + type_sources_rt, ts->get_type_description_sources_func(ts))); + + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_sources_msg); + rosidl_runtime_c__type_description__TypeSource__Sequence__destroy( + type_sources_rt); +} + +TEST(TestTypeDescriptionConversions, type_source_sequence_invalid_input) { + EXPECT_TRUE(NULL == rcl_convert_type_source_sequence_msg_to_runtime(NULL)); + EXPECT_TRUE(NULL == rcl_convert_type_source_sequence_runtime_to_msg(NULL)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} From c8bb895cc337f77f7999c2cab3a6bc80f12e3214 Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Wed, 29 Mar 2023 21:27:05 +0000 Subject: [PATCH 03/14] Add node type cache Signed-off-by: Hans-Joachim Krauch --- rcl/CMakeLists.txt | 1 + rcl/include/rcl/node_type_cache.h | 172 ++++++++++++++++++ rcl/src/rcl/node.c | 18 ++ rcl/src/rcl/node_impl.h | 4 + rcl/src/rcl/node_type_cache.c | 240 ++++++++++++++++++++++++++ rcl/test/CMakeLists.txt | 9 + rcl/test/rcl/test_node_type_cache.cpp | 186 ++++++++++++++++++++ 7 files changed, 630 insertions(+) create mode 100644 rcl/include/rcl/node_type_cache.h create mode 100644 rcl/src/rcl/node_type_cache.c create mode 100644 rcl/test/rcl/test_node_type_cache.cpp diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index e296aac4f..c5092071e 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -59,6 +59,7 @@ set(${PROJECT_NAME}_sources src/rcl/network_flow_endpoints.c src/rcl/node.c src/rcl/node_options.c + src/rcl/node_type_cache.c src/rcl/publisher.c src/rcl/remap.c src/rcl/node_resolve_name.c diff --git a/rcl/include/rcl/node_type_cache.h b/rcl/include/rcl/node_type_cache.h new file mode 100644 index 000000000..01a6d6616 --- /dev/null +++ b/rcl/include/rcl/node_type_cache.h @@ -0,0 +1,172 @@ +// Copyright 2023 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. + +#ifndef RCL__NODE_TYPE_CACHE_H_ +#define RCL__NODE_TYPE_CACHE_H_ + +#include "rcl/node.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rosidl_runtime_c/type_description/type_description__struct.h" +#include "rosidl_runtime_c/type_description/type_source__struct.h" +#include "type_description_interfaces/msg/type_description.h" +#include "type_description_interfaces/msg/type_source.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct rcl_type_info_t +{ + type_description_interfaces__msg__TypeDescription * type_description; + type_description_interfaces__msg__TypeSource__Sequence * type_sources; +} rcl_type_info_t; + +/// Initialize the node's type cache. +/** + * This function initializes hash map of the node's type cache such that types + * can be registered and retrieved. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be initialized + * \return #RCL_RET_OK if the node's type cache was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node); + +/// Finalize the node's type cache. +/** + * This function clears the hash map of the node's type cache and deallocates + * used memory. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be finalized + * \return #RCL_RET_OK if the node's type cache was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node); + +/// Register a type with the node's type cache. +/** + * This function registers the given type, uniquely identified by the type_hash, + * with the node with the node's type cache. Multiple registrations of the same + * type will increment its registration count. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be finalized + * \param[in] type_hash hash of the type + * \param[in] type_description type description struct + * \param[in] type_description_sources type description sources struct + * \return #RCL_RET_OK if the type was successfully registered, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_register_type( + const rcl_node_t * node, const rosidl_type_hash_t * type_hash, + const rosidl_runtime_c__type_description__TypeDescription * type_description, + const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources +); + +/// Unregister a message type from the node's type cache. +/** + * This function uses the given `type_hash` to unregister the associated type in + * the node's type cache. If the type has been registered multiple times, the + * type will only be removed if its registration count reaches 0. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be finalized + * \param[in] type_hash type hash + * \return #RCL_RET_OK if the type was successfully registered, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_unregister_type( + const rcl_node_t * node, const rosidl_type_hash_t * type_hash); + +/// Retrieve type information from the node's type cache. +/** + * This function returns the desired type information from the node's type cache + * + * The `type_info` field must point to an allocated `rcl_type_info_t` object to + * which the type information will be written. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be queried + * \param[in] type_hash type hash + * \param[out] type_info pointer to the type info struct that will be populated + * \return #RCL_RET_OK if type information was retrieved successfully + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_NOT_INIT if node's type cache has not been initialized, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_get_type_info( + const rcl_node_t * node, + const rosidl_type_hash_t * type_hash, + rcl_type_info_t * type_info); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__NODE_TYPE_CACHE_H_ diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index de74a0f82..96491d448 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -53,6 +53,7 @@ extern "C" #include "./context_impl.h" #include "./node_impl.h" +#include "./node_type_cache_init.h" const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR = "ROS_DISABLE_LOANED_MESSAGES"; @@ -198,6 +199,7 @@ rcl_node_init( node->impl->logger_name = NULL; node->impl->fq_name = NULL; node->impl->options = rcl_node_get_default_options(); + node->impl->registered_types_by_type_hash = rcutils_get_zero_initialized_hash_map(); node->context = context; // Initialize node impl. ret = rcl_node_options_copy(options, &(node->impl->options)); @@ -275,6 +277,12 @@ rcl_node_init( // error message already set goto fail; } + // Initialize the node type cache hash map + ret = rcl_node_type_cache_init(node); + if (ret != RCL_RET_OK) { + // error message already set + goto fail; + } // The initialization for the rosout publisher requires the node to be in initialized to a point // that it can create new topic publishers if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { @@ -301,6 +309,11 @@ rcl_node_init( goto cleanup; fail: if (node->impl) { + ret = rcl_node_type_cache_fini(node); + RCUTILS_LOG_ERROR_EXPRESSION_NAMED( + (ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME, + "Failed to fini node_type_cache for node: %i", ret); + if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout && node->impl->logger_name) @@ -378,6 +391,11 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } } + rcl_ret = rcl_node_type_cache_fini(node); + if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) { + RCL_SET_ERROR_MSG("Unable to fini type cache for node."); + result = RCL_RET_ERROR; + } rmw_ret_t rmw_ret = rmw_destroy_node(node->impl->rmw_node_handle); if (rmw_ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); diff --git a/rcl/src/rcl/node_impl.h b/rcl/src/rcl/node_impl.h index 19c089374..e2353ce75 100644 --- a/rcl/src/rcl/node_impl.h +++ b/rcl/src/rcl/node_impl.h @@ -17,6 +17,9 @@ #include "rcl/guard_condition.h" #include "rcl/node_options.h" +#include "rcl/node.h" +#include "rcl/types.h" +#include "rcutils/types/hash_map.h" #include "rmw/types.h" struct rcl_node_impl_s @@ -26,6 +29,7 @@ struct rcl_node_impl_s rcl_guard_condition_t * graph_guard_condition; const char * logger_name; const char * fq_name; + rcutils_hash_map_t registered_types_by_type_hash; }; #endif // RCL__NODE_IMPL_H_ diff --git a/rcl/src/rcl/node_type_cache.c b/rcl/src/rcl/node_type_cache.c new file mode 100644 index 000000000..83bd14318 --- /dev/null +++ b/rcl/src/rcl/node_type_cache.c @@ -0,0 +1,240 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcl/node_type_cache.h" +#include "rcl/type_description_conversions.h" + +#include "rcl/error_handling.h" +#include "rcutils/logging_macros.h" +#include "rcutils/types/hash_map.h" + +#include "./context_impl.h" +#include "./node_impl.h" + +typedef struct rcl_type_info_with_registration_count_t +{ + /// Counter to keep track of registrations + size_t num_registrations; + + /// The actual type info. + rcl_type_info_t type_info; +} rcl_type_info_with_registration_count_t; + +static size_t get_type_hash_hashmap_key(const void * key) +{ + // Reinterpret-cast the first sizeof(size_t) bytes of the hash value + const rosidl_type_hash_t * type_hash = key; + return *(size_t *)type_hash->value; +} + +static int cmp_type_hash(const void * val1, const void * val2) +{ + return memcmp(val1, val2, sizeof(rosidl_type_hash_t)); +} + +rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + + rcutils_ret_t ret = rcutils_hash_map_init( + &node->impl->registered_types_by_type_hash, 2, sizeof(rosidl_type_hash_t), + sizeof(rcl_type_info_with_registration_count_t), + get_type_hash_hashmap_key, cmp_type_hash, + &node->context->impl->allocator); + + if (RCUTILS_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to initialize type cache hash map"); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + + // Clean up any remaining types. + rosidl_type_hash_t key; + rcl_type_info_with_registration_count_t type_info_with_registrations; + rcutils_ret_t hash_map_ret = rcutils_hash_map_get_next_key_and_data( + &node->impl->registered_types_by_type_hash, NULL, &key, + &type_info_with_registrations); + + if (RCUTILS_RET_NOT_INITIALIZED == hash_map_ret) { + return RCL_RET_NOT_INIT; + } + + while (RCUTILS_RET_OK == hash_map_ret) { + hash_map_ret = rcutils_hash_map_unset( + &node->impl->registered_types_by_type_hash, &key); + if (hash_map_ret != RCUTILS_RET_OK) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Failed to clear out type informations [%s] during shutdown; memory " + "will be leaked.", + rcutils_get_error_string().str); + break; + } + + type_description_interfaces__msg__TypeDescription__destroy( + type_info_with_registrations.type_info.type_description); + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_info_with_registrations.type_info.type_sources); + + hash_map_ret = rcutils_hash_map_get_next_key_and_data( + &node->impl->registered_types_by_type_hash, NULL, &key, + &type_info_with_registrations); + } + + rcutils_ret_t rcutils_ret = + rcutils_hash_map_fini(&node->impl->registered_types_by_type_hash); + + return RCUTILS_RET_OK == rcutils_ret ? RCL_RET_OK : RCL_RET_ERROR; +} + +rcl_ret_t rcl_node_type_cache_get_type_info( + const rcl_node_t * node, + const rosidl_type_hash_t * type_hash, + rcl_type_info_t * type_info) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(type_hash, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_info, RCL_RET_INVALID_ARGUMENT); + + rcl_type_info_with_registration_count_t type_info_with_registrations; + + rcutils_ret_t ret = + rcutils_hash_map_get( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info_with_registrations); + if (RCUTILS_RET_OK == ret) { + *type_info = type_info_with_registrations.type_info; + return RCL_RET_OK; + } else if (RCUTILS_RET_NOT_INITIALIZED == ret) { + return RCL_RET_NOT_INIT; + } + + return RCL_RET_ERROR; +} + +rcl_ret_t rcl_node_type_cache_register_type( + const rcl_node_t * node, const rosidl_type_hash_t * type_hash, + const rosidl_runtime_c__type_description__TypeDescription * type_description, + const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(type_hash, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_description, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_description_sources, RCL_RET_INVALID_ARGUMENT); + + rcl_type_info_with_registration_count_t type_info_with_registrations; + + const rcutils_ret_t rcutils_ret = rcutils_hash_map_get( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info_with_registrations); + + if (RCUTILS_RET_OK == rcutils_ret) { + // If the type already exists, we only have to increment the registration + // count. + type_info_with_registrations.num_registrations++; + } else if (RCUTILS_RET_NOT_FOUND == rcutils_ret) { + // First registration of this type + type_info_with_registrations.num_registrations = 1; + + // Convert type description struct to type description message struct. + type_info_with_registrations.type_info.type_description = + rcl_convert_type_description_runtime_to_msg(type_description); + RCL_CHECK_FOR_NULL_WITH_MSG( + type_info_with_registrations.type_info.type_description, + "converting type description struct failed", return RCL_RET_ERROR); + + // Convert type sources struct to type sources message struct. + type_info_with_registrations.type_info.type_sources = + rcl_convert_type_source_sequence_runtime_to_msg(type_description_sources); + RCL_CHECK_FOR_NULL_WITH_MSG( + type_info_with_registrations.type_info.type_description, + "converting type sources struct failed", + type_description_interfaces__msg__TypeDescription__destroy( + type_info_with_registrations.type_info.type_description); + return RCL_RET_ERROR); + } else { + return RCL_RET_ERROR; + } + + // Update the hash map entry. + if (RCUTILS_RET_OK != + rcutils_hash_map_set( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info_with_registrations)) + { + RCL_SET_ERROR_MSG("Failed to update type info"); + type_description_interfaces__msg__TypeDescription__destroy( + type_info_with_registrations.type_info.type_description); + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_info_with_registrations.type_info.type_sources); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t rcl_node_type_cache_unregister_type( + const rcl_node_t * node, const rosidl_type_hash_t * type_hash) +{ + rcl_type_info_with_registration_count_t type_info; + + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(type_hash, RCL_RET_INVALID_ARGUMENT); + + if (RCUTILS_RET_OK != + rcutils_hash_map_get( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info)) + { + RCL_SET_ERROR_MSG("Failed to unregister type"); + return RCL_RET_ERROR; + } + + if (--type_info.num_registrations > 0) { + if (RCUTILS_RET_OK != + rcutils_hash_map_set( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info)) + { + RCL_SET_ERROR_MSG("Failed to update type info"); + return RCL_RET_ERROR; + } + } else { + if (RCUTILS_RET_OK != + rcutils_hash_map_unset( + &node->impl->registered_types_by_type_hash, + type_hash)) + { + RCL_SET_ERROR_MSG("Failed to unregister type info"); + return RCL_RET_ERROR; + } + + type_description_interfaces__msg__TypeDescription__destroy( + type_info.type_info.type_description); + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_info.type_info.type_sources); + } + + return RCL_RET_OK; +} diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 3b9cf43cd..56037e3bf 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -336,6 +336,15 @@ function(test_target_function) AMENT_DEPENDENCIES "test_msgs" ) + rcl_add_custom_gtest(test_node_type_cache${target_suffix} + SRCS rcl/test_node_type_cache.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + # Launch tests rcl_add_custom_executable(service_fixture${target_suffix} diff --git a/rcl/test/rcl/test_node_type_cache.cpp b/rcl/test/rcl/test_node_type_cache.cpp new file mode 100644 index 000000000..378f4c4d5 --- /dev/null +++ b/rcl/test/rcl/test_node_type_cache.cpp @@ -0,0 +1,186 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/node_type_cache.h" +#include "rcl/rcl.h" +#include "rmw/rmw.h" + +#include "test_msgs/msg/basic_types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/env.h" + +#ifdef RMW_IMPLEMENTATION +#define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +#define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +#define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestNodeTypeCacheFixture, RMW_IMPLEMENTATION) + : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) + << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_type_cache_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init( + this->node_ptr, name, "", this->context_ptr, + &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +TEST_F( + CLASSNAME(TestNodeTypeCacheFixture, RMW_IMPLEMENTATION), + test_type_cache_invalid_args) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_type_info_t type_info; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_register_type( + NULL, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_register_type( + this->node_ptr, NULL, + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), NULL, ts->get_type_description_sources_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), ts->get_type_description_func(ts), NULL)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_unregister_type(NULL, ts->get_type_hash_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_unregister_type(this->node_ptr, NULL)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_get_type_info(NULL, ts->get_type_hash_func(ts), &type_info)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_get_type_info( + this->node_ptr, NULL, &type_info)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), NULL)); + rcl_reset_error(); +} + +TEST_F( + CLASSNAME(TestNodeTypeCacheFixture, RMW_IMPLEMENTATION), + test_type_registration_count) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_type_info_t type_info; + + // Register once + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + EXPECT_EQ( + RCL_RET_OK, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), &type_info)); + + // Unregister once and confirm that it got removed from the type cache + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_unregister_type(this->node_ptr, ts->get_type_hash_func(ts))); + EXPECT_EQ( + RCL_RET_ERROR, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), &type_info)); + rcl_reset_error(); + + // Register twice and unregister once. Type info should still be available + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_unregister_type(this->node_ptr, ts->get_type_hash_func(ts))); + EXPECT_EQ( + RCL_RET_OK, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), &type_info)); +} + +TEST_F( + CLASSNAME(TestNodeTypeCacheFixture, RMW_IMPLEMENTATION), + test_invalid_unregistration) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + EXPECT_EQ( + RCL_RET_ERROR, + rcl_node_type_cache_unregister_type(this->node_ptr, ts->get_type_hash_func(ts))); + rcl_reset_error(); +} From fbf8e2ce837fd41af953dfa9a112b16ac3e0c17c Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Thu, 13 Apr 2023 16:21:42 -0300 Subject: [PATCH 04/14] Register subscription, publication, service and action types with node type cache Signed-off-by: Hans-Joachim Krauch --- rcl/src/rcl/publisher.c | 20 +++++++++++++++++ rcl/src/rcl/publisher_impl.h | 1 + rcl/src/rcl/service.c | 22 +++++++++++++++++++ rcl/src/rcl/subscription.c | 22 +++++++++++++++++++ rcl/src/rcl/subscription_impl.h | 1 + rcl_action/src/rcl_action/action_server.c | 19 ++++++++++++++++ .../src/rcl_action/action_server_impl.h | 2 ++ 7 files changed, 87 insertions(+) diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 5a132becd..7a2568698 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -25,6 +25,7 @@ extern "C" #include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rcl/time.h" @@ -127,6 +128,8 @@ rcl_publisher_init( options->qos.avoid_ros_namespace_conventions; // options publisher->impl->options = *options; + // type hash + publisher->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); // context publisher->impl->context = node->context; @@ -137,6 +140,18 @@ rcl_publisher_init( (const void *)publisher->impl->rmw_handle, remapped_topic_name, options->qos.depth); + // Register type. + if (RCL_RET_OK != + rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } + goto cleanup; fail: if (publisher->impl) { @@ -187,6 +202,11 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + // Unregister type + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } allocator.deallocate(publisher->impl, allocator.state); publisher->impl = NULL; } diff --git a/rcl/src/rcl/publisher_impl.h b/rcl/src/rcl/publisher_impl.h index cd5ff2324..26bbb43da 100644 --- a/rcl/src/rcl/publisher_impl.h +++ b/rcl/src/rcl/publisher_impl.h @@ -25,6 +25,7 @@ struct rcl_publisher_impl_s rmw_qos_profile_t actual_qos; rcl_context_t * context; rmw_publisher_t * rmw_handle; + rosidl_type_hash_t type_hash; }; #endif // RCL__PUBLISHER_IMPL_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 0d49289f4..c1b59b5df 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -24,6 +24,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcl/publisher.h" #include "rcl/time.h" #include "rcl/types.h" @@ -47,6 +48,7 @@ struct rcl_service_impl_s rmw_service_t * rmw_handle; rcl_service_event_publisher_t * service_event_publisher; char * remapped_service_name; + rosidl_type_hash_t type_hash; }; rcl_service_t @@ -186,6 +188,8 @@ rcl_service_init( // options service->impl->options = *options; + // type hash + service->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); TRACETOOLS_TRACEPOINT( rcl_service_init, @@ -193,6 +197,18 @@ rcl_service_init( (const void *)node, (const void *)service->impl->rmw_handle, service->impl->remapped_service_name); + // Register type. + if (RCL_RET_OK != + rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for service"); + ret = RCL_RET_ERROR; + goto destroy_service; + } return RCL_RET_OK; @@ -248,6 +264,12 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) result = RCL_RET_ERROR; } + // Unregister type + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(service->impl->remapped_service_name, allocator.state); service->impl->remapped_service_name = NULL; diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 41a7d641b..ecb83d03b 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -23,6 +23,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" #include "rcutils/types/string_array.h" @@ -122,6 +123,8 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; // options subscription->impl->options = *options; + // type hash + subscription->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; TRACETOOLS_TRACEPOINT( @@ -131,6 +134,18 @@ rcl_subscription_init( (const void *)subscription->impl->rmw_handle, remapped_topic_name, options->qos.depth); + // Register type. + if (RCL_RET_OK != + rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } + goto cleanup; fail: if (subscription->impl) { @@ -192,6 +207,13 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) result = RCL_RET_ERROR; } + // Unregister type + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &subscription->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + result = RCL_RET_ERROR; + } + allocator.deallocate(subscription->impl, allocator.state); subscription->impl = NULL; } diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 0fe962ab4..8193421cf 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -24,6 +24,7 @@ struct rcl_subscription_impl_s rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; + rosidl_type_hash_t type_hash; }; #endif // RCL__SUBSCRIPTION_IMPL_H_ diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 2320c9286..bceed1501 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -27,6 +27,7 @@ extern "C" #include "rcl_action/wait.h" #include "rcl/error_handling.h" +#include "rcl/node_type_cache.h" #include "rcl/rcl.h" #include "rcl/time.h" @@ -172,6 +173,8 @@ rcl_action_server_init( // Store reference to clock action_server->impl->clock = clock; + // Store type support + action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); // Initialize Timer ret = rcl_timer_init2( @@ -192,6 +195,18 @@ rcl_action_server_init( ret = RCL_RET_BAD_ALLOC; goto fail; } + + // Register type. + ret = rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support)); + if (RCL_RET_OK != ret) { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for action"); + goto fail; + } + return ret; fail: { @@ -249,6 +264,10 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node) } allocator.deallocate(action_server->impl->goal_handles, allocator.state); action_server->impl->goal_handles = NULL; + // Unregister type + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_server->impl->type_hash)) { + ret = RCL_RET_ERROR; + } // Deallocate struct allocator.deallocate(action_server->impl, allocator.state); action_server->impl = NULL; diff --git a/rcl_action/src/rcl_action/action_server_impl.h b/rcl_action/src/rcl_action/action_server_impl.h index 0002e376c..729a58af5 100644 --- a/rcl_action/src/rcl_action/action_server_impl.h +++ b/rcl_action/src/rcl_action/action_server_impl.h @@ -39,6 +39,8 @@ typedef struct rcl_action_server_impl_s size_t wait_set_cancel_service_index; size_t wait_set_result_service_index; size_t wait_set_expire_timer_index; + // Type hash + rosidl_type_hash_t type_hash; } rcl_action_server_impl_t; From 62cdf1af2196f737992ca2bf8d1abb5a1cdc85cf Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Thu, 13 Apr 2023 16:33:21 -0300 Subject: [PATCH 05/14] Add ~/get_type_description service Signed-off-by: Hans-Joachim Krauch --- rcl/include/rcl/node.h | 51 +++ rcl/src/rcl/node.c | 187 +++++++++- rcl/src/rcl/node_impl.h | 2 + rcl/test/CMakeLists.txt | 9 + .../rcl/test_get_type_description_service.cpp | 318 ++++++++++++++++++ 5 files changed, 566 insertions(+), 1 deletion(-) create mode 100644 rcl/test/rcl/test_get_type_description_service.cpp diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index ce41d0d9b..afe9251bf 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -549,6 +549,57 @@ RCL_PUBLIC rcl_ret_t rcl_get_disable_loaned_message(bool * disable_loaned_message); +/// Initialize the node's ~/get_type_description service. +/** + * This function initializes the node's private ~/get_type_description service + * which can be used to retrieve information about types used by the node's + * publishers, subscribers, services or actions. + * + * Note that this function will be called in `rcl_init_node` if the node option + * `enable_type_description_service` is set to true. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node handle to the node for which to initialize the service + * \return #RCL_RET_OK if the service was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ALREADY_INIT if the service is already initialized, or + * \return #RCL_RET_BAD_ALLOC if memory allocation for the service failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node); + +/// Finalizes the node's ~/get_type_description service. +/** + * This function finalizes the node's private ~/get_type_description service. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be initialized + * \return #RCL_RET_OK if service was deinitialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SERVICE_INVALID if the service is invalid, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_description_service_fini(rcl_node_t * node); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 96491d448..a04e57357 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -30,6 +30,7 @@ extern "C" #include "rcl/localhost.h" #include "rcl/logging.h" #include "rcl/logging_rosout.h" +#include "rcl/node_type_cache.h" #include "rcl/rcl.h" #include "rcl/remap.h" #include "rcl/security.h" @@ -43,17 +44,21 @@ extern "C" #include "rcutils/repl_str.h" #include "rcutils/snprintf.h" #include "rcutils/strdup.h" +#include "rcutils/types/hash_map.h" #include "rmw/error_handling.h" #include "rmw/security_options.h" #include "rmw/rmw.h" #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" +#include "rosidl_runtime_c/string_functions.h" +#include "rosidl_runtime_c/type_description/type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_source__functions.h" #include "tracetools/tracetools.h" +#include "type_description_interfaces/srv/get_type_description.h" #include "./context_impl.h" #include "./node_impl.h" -#include "./node_type_cache_init.h" const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR = "ROS_DISABLE_LOANED_MESSAGES"; @@ -200,6 +205,7 @@ rcl_node_init( node->impl->fq_name = NULL; node->impl->options = rcl_node_get_default_options(); node->impl->registered_types_by_type_hash = rcutils_get_zero_initialized_hash_map(); + node->impl->get_type_description_service = rcl_get_zero_initialized_service(); node->context = context; // Initialize node impl. ret = rcl_node_options_copy(options, &(node->impl->options)); @@ -283,6 +289,14 @@ rcl_node_init( // error message already set goto fail; } + // Initialize ~/get_type_description service + if (node->impl->options.enable_type_description_service) { + ret = rcl_node_type_description_service_init(node); + if (ret != RCL_RET_OK) { + // error message already set + goto fail; + } + } // The initialization for the rosout publisher requires the node to be in initialized to a point // that it can create new topic publishers if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { @@ -309,6 +323,11 @@ rcl_node_init( goto cleanup; fail: if (node->impl) { + ret = rcl_node_type_description_service_fini(node); + RCUTILS_LOG_ERROR_EXPRESSION_NAMED( + (ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME, + "Failed to fini get_type_description service for node: %i", ret); + ret = rcl_node_type_cache_fini(node); RCUTILS_LOG_ERROR_EXPRESSION_NAMED( (ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME, @@ -391,6 +410,11 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } } + rcl_ret = rcl_node_type_description_service_fini(node); + if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) { + RCL_SET_ERROR_MSG("Unable to fini ~/get_type_description service for node."); + result = RCL_RET_ERROR; + } rcl_ret = rcl_node_type_cache_fini(node); if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) { RCL_SET_ERROR_MSG("Unable to fini type cache for node."); @@ -551,6 +575,167 @@ rcl_get_disable_loaned_message(bool * disable_loaned_message) *disable_loaned_message = (strcmp(env_val, "1") == 0); return RCL_RET_OK; } + +static void rcl_node_get_type_description_service_callback( + const void * user_data, + size_t number_of_events) +{ + (void)number_of_events; + rmw_request_id_t request_header; + type_description_interfaces__srv__GetTypeDescription_Request request; + type_description_interfaces__srv__GetTypeDescription_Response response; + rcl_type_info_t type_info; + + const rcl_node_t * node = user_data; + RCL_CHECK_FOR_NULL_WITH_MSG(node, "invalid node handle", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "invalid node", return;); + + if (!(type_description_interfaces__srv__GetTypeDescription_Request__init( + &request) && + type_description_interfaces__srv__GetTypeDescription_Response__init( + &response))) + { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Failed to initialize service request / response."); + goto cleanup; + } + + if (RCL_RET_OK != rcl_take_request( + &node->impl->get_type_description_service, + &request_header, &request)) + { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to take request"); + goto cleanup; + } + + rosidl_type_hash_t type_hash; + if (RCUTILS_RET_OK != + rosidl_parse_type_hash_string(request.type_hash.data, &type_hash)) + { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to parse type hash '%s'", + request.type_hash.data); + response.successful = false; + rosidl_runtime_c__String__assign(&response.failure_reason, "Failed to parse type hash"); + goto send_response; + } + + rcl_ret_t ret = + rcl_node_type_cache_get_type_info(node, &type_hash, &type_info); + + if (RCUTILS_RET_OK == ret) { + response.successful = type_description_interfaces__msg__TypeDescription__copy( + type_info.type_description, &response.type_description); + + if (request.include_type_sources) { + response.successful &= type_description_interfaces__msg__TypeSource__Sequence__copy( + type_info.type_sources, &response.type_sources); + } + + if (!response.successful) { + rosidl_runtime_c__String__assign( + &response.failure_reason, + "Failed to populate response"); + } + } else { + response.successful = false; + rosidl_runtime_c__String__assign( + &response.failure_reason, + "Type not currently used by this node"); + } + +send_response: + if (RCL_RET_OK != rcl_send_response( + &node->impl->get_type_description_service, + &request_header, &response)) + { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to send response"); + } + +cleanup: + type_description_interfaces__srv__GetTypeDescription_Request__fini(&request); + type_description_interfaces__srv__GetTypeDescription_Response__fini( + &response); +} + +rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + + if (rcl_service_is_valid(&node->impl->get_type_description_service)) { + return RCL_RET_ALREADY_INIT; + } + rcl_reset_error(); // Reset the error message set by rcl_service_is_valid() + + char * service_name = NULL; + const rosidl_service_type_support_t * type_support = + ROSIDL_GET_SRV_TYPE_SUPPORT( + type_description_interfaces, srv, + GetTypeDescription); + rcl_service_options_t service_ops = rcl_service_get_default_options(); + rcl_allocator_t allocator = node->context->impl->allocator; + + // Construct service name + rcl_ret_t ret = rcl_node_resolve_name( + node, "~/get_type_description", + allocator, true, true, &service_name); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG( + "Failed to construct ~/get_type_description service name"); + return ret; + } + + // Initialize service + ret = rcl_service_init( + &node->impl->get_type_description_service, node, + type_support, service_name, &service_ops); + allocator.deallocate(service_name, allocator.state); + if (RCL_RET_OK != ret) { + return ret; + } + + // Set service callback + ret = rcl_service_set_on_new_request_callback( + &node->impl->get_type_description_service, + rcl_node_get_type_description_service_callback, node); + if (RCL_RET_OK != ret) { + goto fail_fini_service; + } + + return RCL_RET_OK; + +fail_fini_service: + if (RCL_RET_OK != + rcl_service_fini(&node->impl->get_type_description_service, node)) + { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Failed to deinitialize ~/get_type_description service."); + } + + return ret; +} + +rcl_ret_t rcl_node_type_description_service_fini(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + if (!rcl_service_is_valid(&node->impl->get_type_description_service)) { + rcl_reset_error(); + return RCL_RET_NOT_INIT; + } + + const rcl_ret_t ret = + rcl_service_fini(&node->impl->get_type_description_service, node); + if (RCL_RET_OK == ret) { + node->impl->get_type_description_service = rcl_get_zero_initialized_service(); + } + + return ret; +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/node_impl.h b/rcl/src/rcl/node_impl.h index e2353ce75..8d4f5847e 100644 --- a/rcl/src/rcl/node_impl.h +++ b/rcl/src/rcl/node_impl.h @@ -18,6 +18,7 @@ #include "rcl/guard_condition.h" #include "rcl/node_options.h" #include "rcl/node.h" +#include "rcl/service.h" #include "rcl/types.h" #include "rcutils/types/hash_map.h" #include "rmw/types.h" @@ -30,6 +31,7 @@ struct rcl_node_impl_s const char * logger_name; const char * fq_name; rcutils_hash_map_t registered_types_by_type_hash; + rcl_service_t get_type_description_service; }; #endif // RCL__NODE_IMPL_H_ diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 56037e3bf..1931d0cb2 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -345,6 +345,15 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + rcl_add_custom_gtest(test_get_type_description_service${target_suffix} + SRCS rcl/test_get_type_description_service.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} mimick wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "type_description_interfaces" + ) + # Launch tests rcl_add_custom_executable(service_fixture${target_suffix} diff --git a/rcl/test/rcl/test_get_type_description_service.cpp b/rcl/test/rcl/test_get_type_description_service.cpp new file mode 100644 index 000000000..3acce05ff --- /dev/null +++ b/rcl/test/rcl/test_get_type_description_service.cpp @@ -0,0 +1,318 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl/service.h" +#include "rcl/rcl.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rosidl_runtime_c/string_functions.h" +#include "type_description_interfaces/srv/get_type_description.h" +#include "wait_for_entity_helpers.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +constexpr char GET_TYPE_DESCRIPTION_SRV_TYPE_NAME[] = + "type_description_interfaces/srv/GetTypeDescription"; + +static bool string_in_array(rcutils_string_array_t * array, const char * pattern) +{ + for (size_t i = 0; i < array->size; ++i) { + if (strcmp(array->data[i], pattern) == 0) { + return true; + } + } + return false; +} + +static bool service_exists( + const rcl_node_t * node_ptr, const char * service_name, + const char * service_type) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + + rcl_names_and_types_t * srv_names_and_types = + static_cast(allocator.allocate( + sizeof(rcl_names_and_types_t), + allocator.state)); + if (nullptr == srv_names_and_types) { + return false; + } + EXPECT_EQ(RCL_RET_OK, rcl_names_and_types_init(srv_names_and_types, 0, &allocator)); + srv_names_and_types->names.data = NULL; + srv_names_and_types->names.size = 0; + srv_names_and_types->types = NULL; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_names_and_types_fini(srv_names_and_types)); + allocator.deallocate(srv_names_and_types, allocator.state); + }); + + if ( + RCL_RET_OK != rcl_get_service_names_and_types( + node_ptr, + &allocator, srv_names_and_types)) + { + return false; + } + + if (srv_names_and_types->names.size < 1) { + return false; + } + + const bool srv_name_found = string_in_array( + &srv_names_and_types->names, + service_name); + + if (!srv_name_found) {return false;} + + bool type_name_found = false; + for (size_t i = 0; i < srv_names_and_types->names.size; ++i) { + type_name_found = string_in_array( + &srv_names_and_types->types[i], + service_type); + if (type_name_found) { + break; + } + } + + return type_name_found; +} + +class CLASSNAME (TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + char get_type_description_service_name[256]; + bool enable_get_type_description_service; + + virtual bool get_type_description_service_enabled() const + { + return true; + } + + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_service_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + node_options.enable_type_description_service = get_type_description_service_enabled(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + const char * node_fqn = rcl_node_get_fully_qualified_name(this->node_ptr); + snprintf( + get_type_description_service_name, sizeof(get_type_description_service_name), + "%s/get_type_description", node_fqn); + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +class CLASSNAME (TestGetTypeDescSrvDisabledFixture, + RMW_IMPLEMENTATION) : public CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION) { + bool get_type_description_service_enabled() const override + { + return false; + } +}; + +/* Test service being enabled with node_options. */ +TEST_F( + CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), + test_service_existence_node_options_default) { + EXPECT_TRUE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); +} + +/* Test service being disabled with node_options. */ +TEST_F( + CLASSNAME(TestGetTypeDescSrvDisabledFixture, RMW_IMPLEMENTATION), + test_service_existence_node_options_disabled) { + EXPECT_FALSE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); +} + +/* Test init and fini functions. */ +TEST_F( + CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), + test_service_init_and_fini_functions) { + EXPECT_TRUE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); + EXPECT_EQ(RCL_RET_OK, rcl_node_type_description_service_fini(this->node_ptr)); + EXPECT_FALSE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); + EXPECT_EQ(RCL_RET_NOT_INIT, rcl_node_type_description_service_fini(this->node_ptr)); + + EXPECT_EQ(RCL_RET_OK, rcl_node_type_description_service_init(this->node_ptr)); + EXPECT_TRUE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); + EXPECT_EQ(RCL_RET_ALREADY_INIT, rcl_node_type_description_service_init(this->node_ptr)); +} + +/* Basic nominal test of the ~/get_type_description service. */ +TEST_F(CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), test_service_nominal) { + rcl_ret_t ret; + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + type_description_interfaces, srv, GetTypeDescription); + + // Create client. + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + &client, this->node_ptr, ts, this->get_type_description_service_name, + &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + // Initialize a request. + type_description_interfaces__srv__GetTypeDescription_Request client_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&client_request); + + // Fill the request. We use the GetTypeDescription hash because we know that that type + // is registered. + const rosidl_type_hash_t * type_hash = ts->get_type_hash_func(ts); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * type_hash_str; + EXPECT_EQ(RCUTILS_RET_OK, rosidl_stringify_type_hash(type_hash, allocator, &type_hash_str)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(type_hash_str, allocator.state); + }); + rosidl_runtime_c__String__assign(&client_request.type_hash, type_hash_str); + rosidl_runtime_c__String__assign(&client_request.type_name, GET_TYPE_DESCRIPTION_SRV_TYPE_NAME); + client_request.include_type_sources = false; + + // Send the request. + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + type_description_interfaces__srv__GetTypeDescription_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Initialize the response owned by the client and take the response. + type_description_interfaces__srv__GetTypeDescription_Response client_response; + type_description_interfaces__srv__GetTypeDescription_Response__init(&client_response); + + // Retrieve the response. + rmw_service_info_t header; + ret = rcl_take_response_with_info(&client, &header, &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(client_response.successful, true); + EXPECT_EQ(sequence_number, header.request_id.sequence_number); + + type_description_interfaces__srv__GetTypeDescription_Response__fini(&client_response); +} + +/* Test calling ~/get_type_description service with invalid hash. */ +TEST_F( + CLASSNAME( + TestGetTypeDescSrvEnabledFixture, + RMW_IMPLEMENTATION), test_service_invalid_hash) { + rcl_ret_t ret; + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + type_description_interfaces, srv, GetTypeDescription); + + // Create client. + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + &client, this->node_ptr, ts, this->get_type_description_service_name, + &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + // Initialize a request. + type_description_interfaces__srv__GetTypeDescription_Request client_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&client_request); + + // Fill the request. + rosidl_runtime_c__String__assign(&client_request.type_hash, "foo"); + rosidl_runtime_c__String__assign(&client_request.type_name, "bar"); + client_request.include_type_sources = false; + + // Send the request. + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + type_description_interfaces__srv__GetTypeDescription_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Initialize the response owned by the client and take the response. + type_description_interfaces__srv__GetTypeDescription_Response client_response; + type_description_interfaces__srv__GetTypeDescription_Response__init(&client_response); + + // Retrieve the response. + rmw_service_info_t header; + ret = rcl_take_response_with_info(&client, &header, &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(client_response.successful, false); + EXPECT_GT(strlen(client_response.failure_reason.data), 0); + EXPECT_EQ(sequence_number, header.request_id.sequence_number); + + type_description_interfaces__srv__GetTypeDescription_Response__fini(&client_response); +} From 3459edf9273e85ad494abbe870c927888c6f3dcc Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Thu, 13 Apr 2023 19:56:17 +0000 Subject: [PATCH 06/14] Enable get_type_description service by default Signed-off-by: Hans-Joachim Krauch --- rcl/include/rcl/node_options.h | 2 +- rcl/src/rcl/node.c | 6 ------ rcl/src/rcl/node_options.c | 2 +- rcl/test/rcl/test_graph.cpp | 3 +++ 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index a74fa4bf5..5d749a9ae 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -55,7 +55,7 @@ typedef struct rcl_node_options_s /// Middleware quality of service settings for /rosout. rmw_qos_profile_t rosout_qos; - /// Register the ~/get_type_description service. Defaults to false. + /// Register the ~/get_type_description service. Defaults to true. bool enable_type_description_service; } rcl_node_options_t; diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index a04e57357..fbd9fa0ef 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -306,12 +306,6 @@ rcl_node_init( goto fail; } } - if (node->impl->options.enable_type_description_service) { - RCUTILS_LOG_WARN_NAMED( - ROS_PACKAGE_NAME, - "Requested ~/get_type_description service enabled, but feature is not yet implemented. " - "Service is not created."); - } RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); ret = RCL_RET_OK; TRACETOOLS_TRACEPOINT( diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index 2d97799f1..c1f5de404 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -36,7 +36,7 @@ rcl_node_get_default_options() .arguments = rcl_get_zero_initialized_arguments(), .enable_rosout = true, .rosout_qos = rcl_qos_profile_rosout_default, - .enable_type_description_service = false, + .enable_type_description_service = true, }; return default_options; } diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 4ac5e7a9a..dc5b0dd57 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -84,6 +84,7 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test *this->old_node_ptr = rcl_get_zero_initialized_node(); const char * old_name = "old_node_name"; rcl_node_options_t node_options = rcl_node_get_default_options(); + node_options.enable_type_description_service = false; ret = rcl_node_init(this->old_node_ptr, old_name, "", this->old_context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_shutdown(this->old_context_ptr); // after this, the old_node_ptr should be invalid @@ -939,6 +940,7 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME remote_node_ptr = new rcl_node_t; *remote_node_ptr = rcl_get_zero_initialized_node(); rcl_node_options_t node_options = rcl_node_get_default_options(); + node_options.enable_type_description_service = false; this->remote_context_ptr = new rcl_context_t; *this->remote_context_ptr = rcl_get_zero_initialized_context(); @@ -1414,6 +1416,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi // Graph change since adding new node rcl_node_t node_new = rcl_get_zero_initialized_node(); rcl_node_options_t node_options = rcl_node_get_default_options(); + node_options.enable_type_description_service = false; ret = rcl_node_init(&node_new, "test_graph2", "", context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; From 1a6eec2944017e6e5a5ee20467f29d303f776586 Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Tue, 13 Jun 2023 22:02:26 +0000 Subject: [PATCH 07/14] Revert "Enable get_type_description service by default" This reverts commit 61ca3dc6d4067baeb061a62bcdac97355a4c5e1a. Signed-off-by: Hans-Joachim Krauch --- rcl/include/rcl/node_options.h | 2 +- rcl/src/rcl/node.c | 6 ++++++ rcl/src/rcl/node_options.c | 2 +- rcl/test/rcl/test_graph.cpp | 3 --- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index 5d749a9ae..a74fa4bf5 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -55,7 +55,7 @@ typedef struct rcl_node_options_s /// Middleware quality of service settings for /rosout. rmw_qos_profile_t rosout_qos; - /// Register the ~/get_type_description service. Defaults to true. + /// Register the ~/get_type_description service. Defaults to false. bool enable_type_description_service; } rcl_node_options_t; diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index fbd9fa0ef..a04e57357 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -306,6 +306,12 @@ rcl_node_init( goto fail; } } + if (node->impl->options.enable_type_description_service) { + RCUTILS_LOG_WARN_NAMED( + ROS_PACKAGE_NAME, + "Requested ~/get_type_description service enabled, but feature is not yet implemented. " + "Service is not created."); + } RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); ret = RCL_RET_OK; TRACETOOLS_TRACEPOINT( diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index c1f5de404..2d97799f1 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -36,7 +36,7 @@ rcl_node_get_default_options() .arguments = rcl_get_zero_initialized_arguments(), .enable_rosout = true, .rosout_qos = rcl_qos_profile_rosout_default, - .enable_type_description_service = true, + .enable_type_description_service = false, }; return default_options; } diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index dc5b0dd57..4ac5e7a9a 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -84,7 +84,6 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test *this->old_node_ptr = rcl_get_zero_initialized_node(); const char * old_name = "old_node_name"; rcl_node_options_t node_options = rcl_node_get_default_options(); - node_options.enable_type_description_service = false; ret = rcl_node_init(this->old_node_ptr, old_name, "", this->old_context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_shutdown(this->old_context_ptr); // after this, the old_node_ptr should be invalid @@ -940,7 +939,6 @@ class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEME remote_node_ptr = new rcl_node_t; *remote_node_ptr = rcl_get_zero_initialized_node(); rcl_node_options_t node_options = rcl_node_get_default_options(); - node_options.enable_type_description_service = false; this->remote_context_ptr = new rcl_context_t; *this->remote_context_ptr = rcl_get_zero_initialized_context(); @@ -1416,7 +1414,6 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi // Graph change since adding new node rcl_node_t node_new = rcl_get_zero_initialized_node(); rcl_node_options_t node_options = rcl_node_get_default_options(); - node_options.enable_type_description_service = false; ret = rcl_node_init(&node_new, "test_graph2", "", context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; From 4f301d32b7c4aaa24664c975f9815d689e3d9773 Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Tue, 13 Jun 2023 22:03:27 +0000 Subject: [PATCH 08/14] Remove warning Signed-off-by: Hans-Joachim Krauch --- rcl/src/rcl/node.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index a04e57357..fbd9fa0ef 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -306,12 +306,6 @@ rcl_node_init( goto fail; } } - if (node->impl->options.enable_type_description_service) { - RCUTILS_LOG_WARN_NAMED( - ROS_PACKAGE_NAME, - "Requested ~/get_type_description service enabled, but feature is not yet implemented. " - "Service is not created."); - } RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); ret = RCL_RET_OK; TRACETOOLS_TRACEPOINT( From 398e5af93e91fa24db68211e4c1ce0dd4934961b Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 14 Jun 2023 16:37:55 -0700 Subject: [PATCH 09/14] Optionally initialize and use type cache based on whether type server is used Signed-off-by: Emerson Knapp --- rcl/include/rcl/node_type_cache.h | 21 ++++++++++ rcl/src/rcl/node.c | 49 ++++++++++++----------- rcl/src/rcl/node_type_cache.c | 11 +++++ rcl/src/rcl/publisher.c | 27 +++++++------ rcl/src/rcl/service.c | 29 ++++++++------ rcl/src/rcl/subscription.c | 31 ++++++++------ rcl_action/src/rcl_action/action_server.c | 26 +++++++----- 7 files changed, 122 insertions(+), 72 deletions(-) diff --git a/rcl/include/rcl/node_type_cache.h b/rcl/include/rcl/node_type_cache.h index 01a6d6616..ca8840260 100644 --- a/rcl/include/rcl/node_type_cache.h +++ b/rcl/include/rcl/node_type_cache.h @@ -55,6 +55,27 @@ typedef struct rcl_type_info_t RCL_WARN_UNUSED rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node); +/// Check whether node's type cache has been initialized. +/** + * Based on configuration, the node may optionally disable the type cache, + * this convenience function allows a standardized check for it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache to check + * \return true if the node's type cache was successfully initialized, or + * \return false for any other case. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +bool rcl_node_type_cache_is_valid(const rcl_node_t * node); + /// Finalize the node's type cache. /** * This function clears the hash map of the node's type cache and deallocates diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index fbd9fa0ef..fb8c30883 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -283,14 +283,13 @@ rcl_node_init( // error message already set goto fail; } - // Initialize the node type cache hash map - ret = rcl_node_type_cache_init(node); - if (ret != RCL_RET_OK) { - // error message already set - goto fail; - } - // Initialize ~/get_type_description service + // Initialize ~/get_type_description service and type cache hash map if (node->impl->options.enable_type_description_service) { + ret = rcl_node_type_cache_init(node); + if (ret != RCL_RET_OK) { + // error message already set + goto fail; + } ret = rcl_node_type_description_service_init(node); if (ret != RCL_RET_OK) { // error message already set @@ -317,15 +316,17 @@ rcl_node_init( goto cleanup; fail: if (node->impl) { - ret = rcl_node_type_description_service_fini(node); - RCUTILS_LOG_ERROR_EXPRESSION_NAMED( - (ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME, - "Failed to fini get_type_description service for node: %i", ret); + if (node->impl->options.enable_type_description_service) { + ret = rcl_node_type_description_service_fini(node); + RCUTILS_LOG_ERROR_EXPRESSION_NAMED( + (ret != RCL_RET_OK), ROS_PACKAGE_NAME, + "Failed to fini get_type_description service for node: %i", ret); - ret = rcl_node_type_cache_fini(node); - RCUTILS_LOG_ERROR_EXPRESSION_NAMED( - (ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME, - "Failed to fini node_type_cache for node: %i", ret); + ret = rcl_node_type_cache_fini(node); + RCUTILS_LOG_ERROR_EXPRESSION_NAMED( + (ret != RCL_RET_OK), ROS_PACKAGE_NAME, + "Failed to fini node_type_cache for node: %i", ret); + } if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout && @@ -404,15 +405,15 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } } - rcl_ret = rcl_node_type_description_service_fini(node); - if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) { - RCL_SET_ERROR_MSG("Unable to fini ~/get_type_description service for node."); - result = RCL_RET_ERROR; - } - rcl_ret = rcl_node_type_cache_fini(node); - if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) { - RCL_SET_ERROR_MSG("Unable to fini type cache for node."); - result = RCL_RET_ERROR; + if (node->impl->options.enable_type_description_service) { + if (rcl_node_type_description_service_fini(node) != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Unable to fini ~/get_type_description service for node."); + result = RCL_RET_ERROR; + } + if (rcl_node_type_cache_fini(node) != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Unable to fini type cache for node."); + result = RCL_RET_ERROR; + } } rmw_ret_t rmw_ret = rmw_destroy_node(node->impl->rmw_node_handle); if (rmw_ret != RMW_RET_OK) { diff --git a/rcl/src/rcl/node_type_cache.c b/rcl/src/rcl/node_type_cache.c index 83bd14318..501f52810 100644 --- a/rcl/src/rcl/node_type_cache.c +++ b/rcl/src/rcl/node_type_cache.c @@ -62,6 +62,17 @@ rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node) return RCL_RET_OK; } +bool rcl_node_type_cache_is_valid(const rcl_node_t * node) +{ + if (node == NULL) { + return false; + } + if (node->impl->registered_types_by_type_hash.impl == NULL) { + return false; + } + return true; +} + rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node) { RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 7a2568698..6d1f25120 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -141,15 +141,16 @@ rcl_publisher_init( remapped_topic_name, options->qos.depth); // Register type. - if (RCL_RET_OK != - rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support))) - { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for subscription"); - goto fail; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } } goto cleanup; @@ -203,9 +204,11 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) result = RCL_RET_ERROR; } // Unregister type - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) { - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - result = RCL_RET_ERROR; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } } allocator.deallocate(publisher->impl, allocator.state); publisher->impl = NULL; diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index c1b59b5df..5dcbf3c97 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -198,16 +198,17 @@ rcl_service_init( (const void *)service->impl->rmw_handle, service->impl->remapped_service_name); // Register type. - if (RCL_RET_OK != - rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support))) - { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for service"); - ret = RCL_RET_ERROR; - goto destroy_service; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for service"); + ret = RCL_RET_ERROR; + goto destroy_service; + } } return RCL_RET_OK; @@ -265,9 +266,11 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) } // Unregister type - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) { - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - result = RCL_RET_ERROR; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } } allocator.deallocate(service->impl->remapped_service_name, allocator.state); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index ecb83d03b..197639bc2 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -135,15 +135,16 @@ rcl_subscription_init( remapped_topic_name, options->qos.depth); // Register type. - if (RCL_RET_OK != - rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support))) - { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for subscription"); - goto fail; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } } goto cleanup; @@ -208,10 +209,14 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) } // Unregister type - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &subscription->impl->type_hash)) { - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); - result = RCL_RET_ERROR; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_unregister_type( + node, &subscription->impl->type_hash)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + result = RCL_RET_ERROR; + } } allocator.deallocate(subscription->impl, allocator.state); diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index bceed1501..c4bc2b29b 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -197,14 +197,16 @@ rcl_action_server_init( } // Register type. - ret = rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support)); - if (RCL_RET_OK != ret) { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for action"); - goto fail; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for action"); + goto fail; + } } return ret; @@ -265,8 +267,12 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node) allocator.deallocate(action_server->impl->goal_handles, allocator.state); action_server->impl->goal_handles = NULL; // Unregister type - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_server->impl->type_hash)) { - ret = RCL_RET_ERROR; + if (rcl_node_type_cache_is_valid(node)) { + if (RCL_RET_OK != rcl_node_type_cache_unregister_type( + node, &action_server->impl->type_hash)) + { + ret = RCL_RET_ERROR; + } } // Deallocate struct allocator.deallocate(action_server->impl, allocator.state); From 9d345d64a8c66e31c9f6641b1f0b7743f6af514c Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 15 Jun 2023 12:25:47 -0700 Subject: [PATCH 10/14] Enable type description service in its test Signed-off-by: Emerson Knapp --- rcl/test/rcl/test_node_type_cache.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rcl/test/rcl/test_node_type_cache.cpp b/rcl/test/rcl/test_node_type_cache.cpp index 378f4c4d5..1a91ccda9 100644 --- a/rcl/test/rcl/test_node_type_cache.cpp +++ b/rcl/test/rcl/test_node_type_cache.cpp @@ -59,6 +59,7 @@ class CLASSNAME (TestNodeTypeCacheFixture, RMW_IMPLEMENTATION) *this->node_ptr = rcl_get_zero_initialized_node(); constexpr char name[] = "test_type_cache_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); + node_options.enable_type_description_service = true; ret = rcl_node_init( this->node_ptr, name, "", this->context_ptr, &node_options); From 48a611a2fa1ca32d387082133e99e687866acfee Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Mon, 26 Jun 2023 18:28:19 -0700 Subject: [PATCH 11/14] Next phase rcl get_type_description srv (#5) RCL does not initialize the get_type_description service itself, instead providing a full enough API for full language clients to initialize it and register its callback within their threading/execution framework Signed-off-by: Emerson Knapp --- rcl/include/rcl/node.h | 67 +++++- rcl/include/rcl/node_options.h | 3 - rcl/include/rcl/node_type_cache.h | 4 + rcl/src/rcl/node.c | 197 ++++++++---------- rcl/src/rcl/node_options.c | 2 - rcl/src/rcl/node_type_cache.c | 8 +- rcl/src/rcl/type_description_conversions.c | 10 +- .../rcl/test_get_type_description_service.cpp | 105 +++++++--- rcl/test/rcl/test_node_type_cache.cpp | 1 - .../rcl/test_type_description_conversions.cpp | 38 +++- 10 files changed, 268 insertions(+), 167 deletions(-) diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index afe9251bf..e0213b7de 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -33,9 +33,12 @@ extern "C" #include "rcl/types.h" #include "rcl/visibility_control.h" +#include "type_description_interfaces/srv/get_type_description.h" + extern const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR; typedef struct rcl_node_impl_s rcl_node_impl_t; +typedef struct rcl_service_s rcl_service_t; /// Structure which encapsulates a ROS Node. typedef struct rcl_node_s @@ -551,12 +554,15 @@ rcl_get_disable_loaned_message(bool * disable_loaned_message); /// Initialize the node's ~/get_type_description service. /** - * This function initializes the node's private ~/get_type_description service + * This function initializes the node's ~/get_type_description service * which can be used to retrieve information about types used by the node's * publishers, subscribers, services or actions. * - * Note that this function will be called in `rcl_init_node` if the node option - * `enable_type_description_service` is set to true. + * Note that this will not register any callback for the service, client-level code + * must register rcl_node_type_description_service_handle_request or a custom callback + * to handle incoming requests, via that client's executor/waitset capabilities. + * + * This will initialize the node's type cache, if it has not been initialized already. * *
* Attribute | Adherence @@ -600,6 +606,61 @@ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_node_type_description_service_fini(rcl_node_t * node); + +/// Returns a pointer to the node's ~/get_type_description service. +/** + * On success, sets service_out to the initialized service. + * rcl_node_type_description_service_init must be called before this. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node + * \param[out] service_out Handle to pointer that will be set + * \return #RCL_RET_OK if valid service was returned successfully, or + * \return #RCL_RET_NODE_INVALID if node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NOT_INIT if the service hasn't yet been initialized, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_get_type_description_service( + const rcl_node_t * node, + rcl_service_t ** service_out); + + +/// Process a single pending request to the GetTypeDescription service. +/** + * This function may be called to handle incoming requests by any client starting the service. + * It is not intended to be called directly by users. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node + * \param[in] request_header ID of the incoming request + * \param[in] request Request that came in to the service + * \param[out] response Allocated, uninitialized response to the request + * \return void + */ +RCL_PUBLIC +void rcl_node_type_description_service_handle_request( + rcl_node_t * node, + const rmw_request_id_t * request_header, + const type_description_interfaces__srv__GetTypeDescription_Request * request, + type_description_interfaces__srv__GetTypeDescription_Response * response); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index a74fa4bf5..551d79437 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -54,9 +54,6 @@ typedef struct rcl_node_options_s /// Middleware quality of service settings for /rosout. rmw_qos_profile_t rosout_qos; - - /// Register the ~/get_type_description service. Defaults to false. - bool enable_type_description_service; } rcl_node_options_t; /// Return the default node options in a rcl_node_options_t. diff --git a/rcl/include/rcl/node_type_cache.h b/rcl/include/rcl/node_type_cache.h index ca8840260..f39c48be8 100644 --- a/rcl/include/rcl/node_type_cache.h +++ b/rcl/include/rcl/node_type_cache.h @@ -37,6 +37,8 @@ typedef struct rcl_type_info_t /** * This function initializes hash map of the node's type cache such that types * can be registered and retrieved. + * Note that to correctly capture all types used by a node, this needs to be called + * before any "builtin" publishers or services are created. * *
* Attribute | Adherence @@ -52,6 +54,7 @@ typedef struct rcl_type_info_t * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node); @@ -95,6 +98,7 @@ bool rcl_node_type_cache_is_valid(const rcl_node_t * node); * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ +RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node); diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index fb8c30883..cbdb3c0be 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -283,19 +283,14 @@ rcl_node_init( // error message already set goto fail; } - // Initialize ~/get_type_description service and type cache hash map - if (node->impl->options.enable_type_description_service) { - ret = rcl_node_type_cache_init(node); - if (ret != RCL_RET_OK) { - // error message already set - goto fail; - } - ret = rcl_node_type_description_service_init(node); - if (ret != RCL_RET_OK) { - // error message already set - goto fail; - } + + // To capture all types from builtin topics and services, the type cache needs to be initialized + // before any publishers/subscriptions/services/etc can be created + ret = rcl_node_type_cache_init(node); + if (ret != RCL_RET_OK) { + goto fail; } + // The initialization for the rosout publisher requires the node to be in initialized to a point // that it can create new topic publishers if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { @@ -316,18 +311,6 @@ rcl_node_init( goto cleanup; fail: if (node->impl) { - if (node->impl->options.enable_type_description_service) { - ret = rcl_node_type_description_service_fini(node); - RCUTILS_LOG_ERROR_EXPRESSION_NAMED( - (ret != RCL_RET_OK), ROS_PACKAGE_NAME, - "Failed to fini get_type_description service for node: %i", ret); - - ret = rcl_node_type_cache_fini(node); - RCUTILS_LOG_ERROR_EXPRESSION_NAMED( - (ret != RCL_RET_OK), ROS_PACKAGE_NAME, - "Failed to fini node_type_cache for node: %i", ret); - } - if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout && node->impl->logger_name) @@ -405,15 +388,19 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } } - if (node->impl->options.enable_type_description_service) { - if (rcl_node_type_description_service_fini(node) != RCL_RET_OK) { - RCL_SET_ERROR_MSG("Unable to fini ~/get_type_description service for node."); - result = RCL_RET_ERROR; - } - if (rcl_node_type_cache_fini(node) != RCL_RET_OK) { - RCL_SET_ERROR_MSG("Unable to fini type cache for node."); - result = RCL_RET_ERROR; - } + rcl_ret = rcl_node_type_description_service_fini(node); + if (rcl_ret == RCL_RET_NOT_INIT) { + rcl_reset_error(); + } else if (rcl_ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Unable to fini ~/get_type_description service for node."); + result = RCL_RET_ERROR; + } + rcl_ret = rcl_node_type_cache_fini(node); + if (rcl_ret == RCL_RET_NOT_INIT) { + rcl_reset_error(); + } else if (rcl_ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Unable to fini type cache for node."); + result = RCL_RET_ERROR; } rmw_ret_t rmw_ret = rmw_destroy_node(node->impl->rmw_node_handle); if (rmw_ret != RMW_RET_OK) { @@ -571,87 +558,71 @@ rcl_get_disable_loaned_message(bool * disable_loaned_message) return RCL_RET_OK; } -static void rcl_node_get_type_description_service_callback( - const void * user_data, - size_t number_of_events) +void rcl_node_type_description_service_handle_request( + rcl_node_t * node, + const rmw_request_id_t * request_header, + const type_description_interfaces__srv__GetTypeDescription_Request * request, + type_description_interfaces__srv__GetTypeDescription_Response * response) { - (void)number_of_events; - rmw_request_id_t request_header; - type_description_interfaces__srv__GetTypeDescription_Request request; - type_description_interfaces__srv__GetTypeDescription_Response response; + (void)request_header; rcl_type_info_t type_info; - - const rcl_node_t * node = user_data; RCL_CHECK_FOR_NULL_WITH_MSG(node, "invalid node handle", return;); RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "invalid node", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(request_header, "invalid request header", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(request, "null request pointer", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(response, "null response pointer", return;); - if (!(type_description_interfaces__srv__GetTypeDescription_Request__init( - &request) && - type_description_interfaces__srv__GetTypeDescription_Response__init( - &response))) - { + if (!type_description_interfaces__srv__GetTypeDescription_Response__init(response)) { RCUTILS_LOG_ERROR_NAMED( ROS_PACKAGE_NAME, - "Failed to initialize service request / response."); - goto cleanup; - } - - if (RCL_RET_OK != rcl_take_request( - &node->impl->get_type_description_service, - &request_header, &request)) - { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to take request"); - goto cleanup; + "Failed to initialize service response."); + return; } + response->successful = false; rosidl_type_hash_t type_hash; if (RCUTILS_RET_OK != - rosidl_parse_type_hash_string(request.type_hash.data, &type_hash)) + rosidl_parse_type_hash_string(request->type_hash.data, &type_hash)) { RCUTILS_LOG_ERROR_NAMED( ROS_PACKAGE_NAME, "Failed to parse type hash '%s'", - request.type_hash.data); - response.successful = false; - rosidl_runtime_c__String__assign(&response.failure_reason, "Failed to parse type hash"); - goto send_response; + request->type_hash.data); + rosidl_runtime_c__String__assign( + &response->failure_reason, + "Failed to parse type hash"); + return; } - rcl_ret_t ret = - rcl_node_type_cache_get_type_info(node, &type_hash, &type_info); - - if (RCUTILS_RET_OK == ret) { - response.successful = type_description_interfaces__msg__TypeDescription__copy( - type_info.type_description, &response.type_description); - - if (request.include_type_sources) { - response.successful &= type_description_interfaces__msg__TypeSource__Sequence__copy( - type_info.type_sources, &response.type_sources); - } - - if (!response.successful) { - rosidl_runtime_c__String__assign( - &response.failure_reason, - "Failed to populate response"); - } - } else { - response.successful = false; + if (RCUTILS_RET_OK != + rcl_node_type_cache_get_type_info(node, &type_hash, &type_info)) + { rosidl_runtime_c__String__assign( - &response.failure_reason, - "Type not currently used by this node"); + &response->failure_reason, + "Type not currently in use by this node"); + return; } -send_response: - if (RCL_RET_OK != rcl_send_response( - &node->impl->get_type_description_service, - &request_header, &response)) + if (!type_description_interfaces__msg__TypeDescription__copy( + type_info.type_description, &response->type_description)) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to send response"); + rosidl_runtime_c__String__assign( + &response->failure_reason, + "Failed to populate TypeDescription to response."); + return; } -cleanup: - type_description_interfaces__srv__GetTypeDescription_Request__fini(&request); - type_description_interfaces__srv__GetTypeDescription_Response__fini( - &response); + if (request->include_type_sources) { + if (!type_description_interfaces__msg__TypeSource__Sequence__copy( + type_info.type_sources, &response->type_sources)) + { + rosidl_runtime_c__String__assign( + &response->failure_reason, + "Failed to populate TypeSource_Sequence to response."); + return; + } + } + + response->successful = true; } rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node) @@ -659,6 +630,8 @@ rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node) RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + rcl_ret_t ret; + if (rcl_service_is_valid(&node->impl->get_type_description_service)) { return RCL_RET_ALREADY_INIT; } @@ -673,7 +646,7 @@ rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node) rcl_allocator_t allocator = node->context->impl->allocator; // Construct service name - rcl_ret_t ret = rcl_node_resolve_name( + ret = rcl_node_resolve_name( node, "~/get_type_description", allocator, true, true, &service_name); if (RCL_RET_OK != ret) { @@ -687,28 +660,6 @@ rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node) &node->impl->get_type_description_service, node, type_support, service_name, &service_ops); allocator.deallocate(service_name, allocator.state); - if (RCL_RET_OK != ret) { - return ret; - } - - // Set service callback - ret = rcl_service_set_on_new_request_callback( - &node->impl->get_type_description_service, - rcl_node_get_type_description_service_callback, node); - if (RCL_RET_OK != ret) { - goto fail_fini_service; - } - - return RCL_RET_OK; - -fail_fini_service: - if (RCL_RET_OK != - rcl_service_fini(&node->impl->get_type_description_service, node)) - { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "Failed to deinitialize ~/get_type_description service."); - } return ret; } @@ -731,6 +682,22 @@ rcl_ret_t rcl_node_type_description_service_fini(rcl_node_t * node) return ret; } +rcl_ret_t rcl_node_get_type_description_service( + const rcl_node_t * node, + rcl_service_t ** service_out) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(service_out, RCL_RET_SERVICE_INVALID); + + if (!rcl_service_is_valid(&node->impl->get_type_description_service)) { + return RCL_RET_NOT_INIT; + } + + *service_out = &node->impl->get_type_description_service; + return RCL_RET_OK; +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index 2d97799f1..34408f18b 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -36,7 +36,6 @@ rcl_node_get_default_options() .arguments = rcl_get_zero_initialized_arguments(), .enable_rosout = true, .rosout_qos = rcl_qos_profile_rosout_default, - .enable_type_description_service = false, }; return default_options; } @@ -62,7 +61,6 @@ rcl_node_options_copy( options_out->use_global_arguments = options->use_global_arguments; options_out->enable_rosout = options->enable_rosout; options_out->rosout_qos = options->rosout_qos; - options_out->enable_type_description_service = options->enable_type_description_service; if (NULL != options->arguments.impl) { return rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); } diff --git a/rcl/src/rcl/node_type_cache.c b/rcl/src/rcl/node_type_cache.c index 501f52810..dd4185aa8 100644 --- a/rcl/src/rcl/node_type_cache.c +++ b/rcl/src/rcl/node_type_cache.c @@ -47,6 +47,10 @@ rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node) { RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + if (NULL != node->impl->registered_types_by_type_hash.impl) { + // already initialized + return RCL_RET_OK; + } rcutils_ret_t ret = rcutils_hash_map_init( &node->impl->registered_types_by_type_hash, 2, sizeof(rosidl_type_hash_t), @@ -178,7 +182,7 @@ rcl_ret_t rcl_node_type_cache_register_type( type_info_with_registrations.type_info.type_sources = rcl_convert_type_source_sequence_runtime_to_msg(type_description_sources); RCL_CHECK_FOR_NULL_WITH_MSG( - type_info_with_registrations.type_info.type_description, + type_info_with_registrations.type_info.type_sources, "converting type sources struct failed", type_description_interfaces__msg__TypeDescription__destroy( type_info_with_registrations.type_info.type_description); @@ -218,7 +222,7 @@ rcl_ret_t rcl_node_type_cache_unregister_type( &node->impl->registered_types_by_type_hash, type_hash, &type_info)) { - RCL_SET_ERROR_MSG("Failed to unregister type"); + RCL_SET_ERROR_MSG("Failed to unregister type, hash not present in map."); return RCL_RET_ERROR; } diff --git a/rcl/src/rcl/type_description_conversions.c b/rcl/src/rcl/type_description_conversions.c index 977269168..f3a041868 100644 --- a/rcl/src/rcl/type_description_conversions.c +++ b/rcl/src/rcl/type_description_conversions.c @@ -252,10 +252,12 @@ rcl_convert_type_source_sequence_runtime_to_msg( goto fail; } // raw_file_contents - if (!rosidl_runtime_c__String__copy( - &(runtime_type_sources->data[i].raw_file_contents), &(out->data[i].raw_file_contents))) - { - goto fail; + if (runtime_type_sources->data[i].raw_file_contents.size > 0) { + if (!rosidl_runtime_c__String__copy( + &(runtime_type_sources->data[i].raw_file_contents), &(out->data[i].raw_file_contents))) + { + goto fail; + } } } diff --git a/rcl/test/rcl/test_get_type_description_service.cpp b/rcl/test/rcl/test_get_type_description_service.cpp index 3acce05ff..1280b4d28 100644 --- a/rcl/test/rcl/test_get_type_description_service.cpp +++ b/rcl/test/rcl/test_get_type_description_service.cpp @@ -22,6 +22,8 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rosidl_runtime_c/string_functions.h" #include "type_description_interfaces/srv/get_type_description.h" + +#include "node_impl.h" // NOLINT #include "wait_for_entity_helpers.hpp" #ifdef RMW_IMPLEMENTATION @@ -99,13 +101,12 @@ static bool service_exists( return type_name_found; } -class CLASSNAME (TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION) : public ::testing::Test +class CLASSNAME (TestGetTypeDescSrvFixture, RMW_IMPLEMENTATION) : public ::testing::Test { public: rcl_context_t * context_ptr; rcl_node_t * node_ptr; char get_type_description_service_name[256]; - bool enable_get_type_description_service; virtual bool get_type_description_service_enabled() const { @@ -132,9 +133,10 @@ class CLASSNAME (TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION) : public *this->node_ptr = rcl_get_zero_initialized_node(); const char * name = "test_service_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); - node_options.enable_type_description_service = get_type_description_service_enabled(); ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_type_description_service_init(node_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; const char * node_fqn = rcl_node_get_fully_qualified_name(this->node_ptr); snprintf( @@ -155,37 +157,10 @@ class CLASSNAME (TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION) : public } }; -class CLASSNAME (TestGetTypeDescSrvDisabledFixture, - RMW_IMPLEMENTATION) : public CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION) { - bool get_type_description_service_enabled() const override - { - return false; - } -}; - -/* Test service being enabled with node_options. */ -TEST_F( - CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), - test_service_existence_node_options_default) { - EXPECT_TRUE( - service_exists( - this->node_ptr, this->get_type_description_service_name, - GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); -} - -/* Test service being disabled with node_options. */ -TEST_F( - CLASSNAME(TestGetTypeDescSrvDisabledFixture, RMW_IMPLEMENTATION), - test_service_existence_node_options_disabled) { - EXPECT_FALSE( - service_exists( - this->node_ptr, this->get_type_description_service_name, - GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); -} /* Test init and fini functions. */ TEST_F( - CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), + CLASSNAME(TestGetTypeDescSrvFixture, RMW_IMPLEMENTATION), test_service_init_and_fini_functions) { EXPECT_TRUE( service_exists( @@ -207,7 +182,7 @@ TEST_F( } /* Basic nominal test of the ~/get_type_description service. */ -TEST_F(CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), test_service_nominal) { +TEST_F(CLASSNAME(TestGetTypeDescSrvFixture, RMW_IMPLEMENTATION), test_service_nominal) { rcl_ret_t ret; const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( type_description_interfaces, srv, GetTypeDescription); @@ -250,6 +225,38 @@ TEST_F(CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), test_ser type_description_interfaces__srv__GetTypeDescription_Request__fini(&client_request); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // This scope simulates handling request in a different context + { + auto service = &node_ptr->impl->get_type_description_service; + ASSERT_TRUE(wait_for_service_to_be_ready(service, context_ptr, 10, 100)); + + type_description_interfaces__srv__GetTypeDescription_Response service_response; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Response__fini(&service_response); + }); + + type_description_interfaces__srv__GetTypeDescription_Request service_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Request__fini(&service_request); + }); + rmw_service_info_t header; + ret = rcl_take_request_with_info(service, &header, &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_node_type_description_service_handle_request( + node_ptr, + &header.request_id, + &service_request, + &service_response); + + ret = rcl_send_response(service, &header.request_id, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_client_to_be_ready(&client, context_ptr, 10, 100)); // Initialize the response owned by the client and take the response. type_description_interfaces__srv__GetTypeDescription_Response client_response; type_description_interfaces__srv__GetTypeDescription_Response__init(&client_response); @@ -267,7 +274,7 @@ TEST_F(CLASSNAME(TestGetTypeDescSrvEnabledFixture, RMW_IMPLEMENTATION), test_ser /* Test calling ~/get_type_description service with invalid hash. */ TEST_F( CLASSNAME( - TestGetTypeDescSrvEnabledFixture, + TestGetTypeDescSrvFixture, RMW_IMPLEMENTATION), test_service_invalid_hash) { rcl_ret_t ret; const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( @@ -302,6 +309,38 @@ TEST_F( type_description_interfaces__srv__GetTypeDescription_Request__fini(&client_request); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // This scope simulates handling request in a different context + { + auto service = &node_ptr->impl->get_type_description_service; + ASSERT_TRUE(wait_for_service_to_be_ready(service, context_ptr, 10, 100)); + + type_description_interfaces__srv__GetTypeDescription_Response service_response; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Response__fini(&service_response); + }); + + type_description_interfaces__srv__GetTypeDescription_Request service_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Request__fini(&service_request); + }); + rmw_service_info_t header; + ret = rcl_take_request_with_info(service, &header, &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_node_type_description_service_handle_request( + node_ptr, + &header.request_id, + &service_request, + &service_response); + + ret = rcl_send_response(service, &header.request_id, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_client_to_be_ready(&client, context_ptr, 10, 100)); // Initialize the response owned by the client and take the response. type_description_interfaces__srv__GetTypeDescription_Response client_response; type_description_interfaces__srv__GetTypeDescription_Response__init(&client_response); diff --git a/rcl/test/rcl/test_node_type_cache.cpp b/rcl/test/rcl/test_node_type_cache.cpp index 1a91ccda9..378f4c4d5 100644 --- a/rcl/test/rcl/test_node_type_cache.cpp +++ b/rcl/test/rcl/test_node_type_cache.cpp @@ -59,7 +59,6 @@ class CLASSNAME (TestNodeTypeCacheFixture, RMW_IMPLEMENTATION) *this->node_ptr = rcl_get_zero_initialized_node(); constexpr char name[] = "test_type_cache_node"; rcl_node_options_t node_options = rcl_node_get_default_options(); - node_options.enable_type_description_service = true; ret = rcl_node_init( this->node_ptr, name, "", this->context_ptr, &node_options); diff --git a/rcl/test/rcl/test_type_description_conversions.cpp b/rcl/test/rcl/test_type_description_conversions.cpp index f0be61e06..71a698f91 100644 --- a/rcl/test/rcl/test_type_description_conversions.cpp +++ b/rcl/test/rcl/test_type_description_conversions.cpp @@ -18,11 +18,12 @@ #include "rosidl_runtime_c/message_type_support_struct.h" #include "rosidl_runtime_c/type_description/type_description__functions.h" #include "rosidl_runtime_c/type_description/type_source__functions.h" -#include "test_msgs/msg/basic_types.h" +#include "test_msgs/msg/constants.h" +#include "test_msgs/srv/basic_types.h" TEST(TestTypeDescriptionConversions, type_description_conversion_round_trip) { const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Constants); type_description_interfaces__msg__TypeDescription * type_description_msg = rcl_convert_type_description_runtime_to_msg(ts->get_type_description_func(ts)); @@ -49,16 +50,35 @@ TEST(TestTypeDescriptionConversions, type_description_invalid_input) { TEST(TestTypeDescriptionConversions, type_source_sequence_conversion_round_trip) { const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Constants); + const rosidl_runtime_c__type_description__TypeSource__Sequence * original_sources = + ts->get_type_description_sources_func(ts); type_description_interfaces__msg__TypeSource__Sequence * type_sources_msg = - rcl_convert_type_source_sequence_runtime_to_msg(ts->get_type_description_sources_func(ts)); + rcl_convert_type_source_sequence_runtime_to_msg(original_sources); EXPECT_TRUE(NULL != type_sources_msg); + ASSERT_EQ(1, type_sources_msg->size); + { + auto source = type_sources_msg->data[0]; + std::string type_name = source.type_name.data; + EXPECT_GT(source.raw_file_contents.size, 0); + std::string encoding = source.encoding.data; + EXPECT_EQ(encoding, "msg"); + } rosidl_runtime_c__type_description__TypeSource__Sequence * type_sources_rt = rcl_convert_type_source_sequence_msg_to_runtime(type_sources_msg); EXPECT_TRUE(NULL != type_sources_rt); + ASSERT_EQ(1, type_sources_rt->size); + { + auto source = type_sources_rt->data[0]; + std::string type_name = source.type_name.data; + EXPECT_GT(source.raw_file_contents.size, 0); + std::string encoding = source.encoding.data; + EXPECT_EQ(encoding, "msg"); + } + EXPECT_TRUE( rosidl_runtime_c__type_description__TypeSource__Sequence__are_equal( type_sources_rt, ts->get_type_description_sources_func(ts))); @@ -69,6 +89,16 @@ TEST(TestTypeDescriptionConversions, type_source_sequence_conversion_round_trip) type_sources_rt); } +TEST(TestTypeDescriptionConversions, actually_empty_sources_ok) { + // Implicit definition will always be empty + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, srv, BasicTypes_Request); + + const auto * sources = ts->get_type_description_sources_func(ts); + auto * msg = rcl_convert_type_source_sequence_runtime_to_msg(sources); + ASSERT_NE(nullptr, msg); +} + TEST(TestTypeDescriptionConversions, type_source_sequence_invalid_input) { EXPECT_TRUE(NULL == rcl_convert_type_source_sequence_msg_to_runtime(NULL)); EXPECT_TRUE(NULL == rcl_convert_type_source_sequence_runtime_to_msg(NULL)); From fdff80ed5f17e361355ca9b3ab45fc52ed535a45 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 3 Jul 2023 12:58:46 -0700 Subject: [PATCH 12/14] Unconditional type registrations, add to service/action client, minor formatting touchups Signed-off-by: Emerson Knapp --- rcl/include/rcl/node_type_cache.h | 23 ------------ rcl/src/rcl/client.c | 20 ++++++++++ rcl/src/rcl/node.c | 17 ++++----- rcl/src/rcl/node_type_cache.c | 14 +------ rcl/src/rcl/publisher.c | 33 +++++++---------- rcl/src/rcl/service.c | 35 ++++++++---------- rcl/src/rcl/subscription.c | 37 ++++++++----------- .../rcl/test_get_type_description_service.cpp | 3 ++ rcl_action/src/rcl_action/action_client.c | 16 ++++++++ .../src/rcl_action/action_client_impl.h | 1 + rcl_action/src/rcl_action/action_server.c | 32 ++++++---------- .../src/rcl_action/action_server_impl.h | 1 - 12 files changed, 105 insertions(+), 127 deletions(-) diff --git a/rcl/include/rcl/node_type_cache.h b/rcl/include/rcl/node_type_cache.h index f39c48be8..c9f21b117 100644 --- a/rcl/include/rcl/node_type_cache.h +++ b/rcl/include/rcl/node_type_cache.h @@ -54,31 +54,9 @@ typedef struct rcl_type_info_t * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ -RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node); -/// Check whether node's type cache has been initialized. -/** - * Based on configuration, the node may optionally disable the type cache, - * this convenience function allows a standardized check for it. - * - *
- * Attribute | Adherence - * ------------------ | ------------- - * Allocates Memory | No - * Thread-Safe | No - * Uses Atomics | No - * Lock-Free | Yes - * - * \param[in] node the handle to the node whose type cache to check - * \return true if the node's type cache was successfully initialized, or - * \return false for any other case. - */ -RCL_PUBLIC -RCL_WARN_UNUSED -bool rcl_node_type_cache_is_valid(const rcl_node_t * node); - /// Finalize the node's type cache. /** * This function clears the hash map of the node's type cache and deallocates @@ -98,7 +76,6 @@ bool rcl_node_type_cache_is_valid(const rcl_node_t * node); * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or * \return #RCL_RET_ERROR if an unspecified error occurs. */ -RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node); diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 8dca90ab7..4e9f88435 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -24,6 +24,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcl/publisher.h" #include "rcl/time.h" #include "rcutils/logging_macros.h" @@ -48,6 +49,7 @@ struct rcl_client_impl_s atomic_int_least64_t sequence_number; rcl_service_event_publisher_t * service_event_publisher; char * remapped_service_name; + rosidl_type_hash_t type_hash; }; rcl_client_t @@ -175,6 +177,19 @@ rcl_client_init( // options client->impl->options = *options; atomic_init(&client->impl->sequence_number, 0); + + client->impl->type_hash = *type_support->get_type_hash_func(type_support); + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for client"); + ret = RCL_RET_ERROR; + goto destroy_client; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); TRACETOOLS_TRACEPOINT( rcl_client_init, @@ -236,6 +251,11 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) result = RCL_RET_ERROR; } + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &client->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(client->impl->remapped_service_name, allocator.state); client->impl->remapped_service_name = NULL; diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index cbdb3c0be..435af671a 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -321,6 +321,12 @@ rcl_node_init( ROS_PACKAGE_NAME, "Failed to fini publisher for node: %i", ret); allocator->deallocate((char *)node->impl->logger_name, allocator->state); } + if (node->impl->registered_types_by_type_hash.impl) { + ret = rcl_node_type_cache_fini(node); + RCUTILS_LOG_ERROR_EXPRESSION_NAMED( + (ret != RCL_RET_OK), + ROS_PACKAGE_NAME, "Failed to fini type cache for node: %i", ret); + } if (node->impl->fq_name) { allocator->deallocate((char *)node->impl->fq_name, allocator->state); } @@ -388,17 +394,8 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } } - rcl_ret = rcl_node_type_description_service_fini(node); - if (rcl_ret == RCL_RET_NOT_INIT) { - rcl_reset_error(); - } else if (rcl_ret != RCL_RET_OK) { - RCL_SET_ERROR_MSG("Unable to fini ~/get_type_description service for node."); - result = RCL_RET_ERROR; - } rcl_ret = rcl_node_type_cache_fini(node); - if (rcl_ret == RCL_RET_NOT_INIT) { - rcl_reset_error(); - } else if (rcl_ret != RCL_RET_OK) { + if (rcl_ret != RCL_RET_OK) { RCL_SET_ERROR_MSG("Unable to fini type cache for node."); result = RCL_RET_ERROR; } diff --git a/rcl/src/rcl/node_type_cache.c b/rcl/src/rcl/node_type_cache.c index dd4185aa8..0d3734a53 100644 --- a/rcl/src/rcl/node_type_cache.c +++ b/rcl/src/rcl/node_type_cache.c @@ -66,17 +66,6 @@ rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node) return RCL_RET_OK; } -bool rcl_node_type_cache_is_valid(const rcl_node_t * node) -{ - if (node == NULL) { - return false; - } - if (node->impl->registered_types_by_type_hash.impl == NULL) { - return false; - } - return true; -} - rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node) { RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); @@ -209,7 +198,8 @@ rcl_ret_t rcl_node_type_cache_register_type( } rcl_ret_t rcl_node_type_cache_unregister_type( - const rcl_node_t * node, const rosidl_type_hash_t * type_hash) + const rcl_node_t * node, + const rosidl_type_hash_t * type_hash) { rcl_type_info_with_registration_count_t type_info; diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 6d1f25120..d5cd4cd12 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -128,8 +128,18 @@ rcl_publisher_init( options->qos.avoid_ros_namespace_conventions; // options publisher->impl->options = *options; - // type hash + publisher->impl->type_hash = *type_support->get_type_hash_func(type_support); + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); // context publisher->impl->context = node->context; @@ -140,18 +150,6 @@ rcl_publisher_init( (const void *)publisher->impl->rmw_handle, remapped_topic_name, options->qos.depth); - // Register type. - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support))) - { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for subscription"); - goto fail; - } - } goto cleanup; fail: @@ -203,12 +201,9 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - // Unregister type - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) { - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - result = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; } allocator.deallocate(publisher->impl, allocator.state); publisher->impl = NULL; diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 5dcbf3c97..17bd1e1da 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -188,8 +188,19 @@ rcl_service_init( // options service->impl->options = *options; - // type hash + service->impl->type_hash = *type_support->get_type_hash_func(type_support); + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for service"); + ret = RCL_RET_ERROR; + goto destroy_service; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); TRACETOOLS_TRACEPOINT( rcl_service_init, @@ -197,19 +208,6 @@ rcl_service_init( (const void *)node, (const void *)service->impl->rmw_handle, service->impl->remapped_service_name); - // Register type. - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support))) - { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for service"); - ret = RCL_RET_ERROR; - goto destroy_service; - } - } return RCL_RET_OK; @@ -265,12 +263,9 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) result = RCL_RET_ERROR; } - // Unregister type - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) { - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - result = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; } allocator.deallocate(service->impl->remapped_service_name, allocator.state); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 197639bc2..b3bb2eb26 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -123,8 +123,18 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; // options subscription->impl->options = *options; - // type hash + subscription->impl->type_hash = *type_support->get_type_hash_func(type_support); + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; TRACETOOLS_TRACEPOINT( @@ -134,18 +144,6 @@ rcl_subscription_init( (const void *)subscription->impl->rmw_handle, remapped_topic_name, options->qos.depth); - // Register type. - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support))) - { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for subscription"); - goto fail; - } - } goto cleanup; fail: @@ -208,15 +206,10 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) result = RCL_RET_ERROR; } - // Unregister type - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_unregister_type( - node, &subscription->impl->type_hash)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); - result = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &subscription->impl->type_hash)) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + result = RCL_RET_ERROR; } allocator.deallocate(subscription->impl, allocator.state); diff --git a/rcl/test/rcl/test_get_type_description_service.cpp b/rcl/test/rcl/test_get_type_description_service.cpp index 1280b4d28..e2571e75c 100644 --- a/rcl/test/rcl/test_get_type_description_service.cpp +++ b/rcl/test/rcl/test_get_type_description_service.cpp @@ -14,12 +14,15 @@ #include +#include + #include "rcl/error_handling.h" #include "rcl/graph.h" #include "rcl/service.h" #include "rcl/rcl.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcutils/types/string_array.h" #include "rosidl_runtime_c/string_functions.h" #include "type_description_interfaces/srv/get_type_description.h" diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index d999f1e01..87ed39f21 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -28,6 +28,7 @@ extern "C" #include "rcl/client.h" #include "rcl/error_handling.h" #include "rcl/graph.h" +#include "rcl/node_type_cache.h" #include "rcl/subscription.h" #include "rcl/types.h" #include "rcl/wait.h" @@ -91,6 +92,9 @@ _rcl_action_client_fini_impl( if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->status_subscription, node)) { ret = RCL_RET_ERROR; } + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_client->impl->type_hash)) { + ret = RCL_RET_ERROR; + } allocator.deallocate(action_client->impl->action_name, allocator.state); allocator.deallocate(action_client->impl, allocator.state); action_client->impl = NULL; @@ -222,6 +226,18 @@ rcl_action_client_init( SUBSCRIPTION_INIT(feedback); SUBSCRIPTION_INIT(status); + action_client->impl->type_hash = *type_support->get_type_hash_func(type_support); + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for action"); + goto fail; + } + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); return ret; fail: diff --git a/rcl_action/src/rcl_action/action_client_impl.h b/rcl_action/src/rcl_action/action_client_impl.h index 777416bfd..f1d430a8d 100644 --- a/rcl_action/src/rcl_action/action_client_impl.h +++ b/rcl_action/src/rcl_action/action_client_impl.h @@ -33,6 +33,7 @@ typedef struct rcl_action_client_impl_s size_t wait_set_result_client_index; size_t wait_set_feedback_subscription_index; size_t wait_set_status_subscription_index; + rosidl_type_hash_t type_hash; } rcl_action_client_impl_t; diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index c4bc2b29b..84991089f 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -173,8 +173,6 @@ rcl_action_server_init( // Store reference to clock action_server->impl->clock = clock; - // Store type support - action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); // Initialize Timer ret = rcl_timer_init2( @@ -196,17 +194,16 @@ rcl_action_server_init( goto fail; } - // Register type. - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_register_type( - node, type_support->get_type_hash_func(type_support), - type_support->get_type_description_func(type_support), - type_support->get_type_description_sources_func(type_support))) - { - rcutils_reset_error(); - RCL_SET_ERROR_MSG("Failed to register type for action"); - goto fail; - } + // Store type hash + action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for action"); + goto fail; } return ret; @@ -266,13 +263,8 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node) } allocator.deallocate(action_server->impl->goal_handles, allocator.state); action_server->impl->goal_handles = NULL; - // Unregister type - if (rcl_node_type_cache_is_valid(node)) { - if (RCL_RET_OK != rcl_node_type_cache_unregister_type( - node, &action_server->impl->type_hash)) - { - ret = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_server->impl->type_hash)) { + ret = RCL_RET_ERROR; } // Deallocate struct allocator.deallocate(action_server->impl, allocator.state); diff --git a/rcl_action/src/rcl_action/action_server_impl.h b/rcl_action/src/rcl_action/action_server_impl.h index 729a58af5..572f799f6 100644 --- a/rcl_action/src/rcl_action/action_server_impl.h +++ b/rcl_action/src/rcl_action/action_server_impl.h @@ -39,7 +39,6 @@ typedef struct rcl_action_server_impl_s size_t wait_set_cancel_service_index; size_t wait_set_result_service_index; size_t wait_set_expire_timer_index; - // Type hash rosidl_type_hash_t type_hash; } rcl_action_server_impl_t; From 37f94368362b58ca9875f33a229be39b4e2d78cd Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 3 Jul 2023 16:35:13 -0700 Subject: [PATCH 13/14] Avoid attempting unregister if no successful registration occurred, reducing error logging on error-case shutdown Signed-off-by: Emerson Knapp --- rcl/src/rcl/client.c | 7 +++++-- rcl/src/rcl/publisher.c | 11 +++++++---- rcl/src/rcl/service.c | 7 +++++-- rcl/src/rcl/subscription.c | 7 +++++-- rcl_action/src/rcl_action/action_client.c | 11 +++++++---- rcl_action/src/rcl_action/action_server.c | 8 ++++++-- 6 files changed, 35 insertions(+), 16 deletions(-) diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 4e9f88435..0a30427ed 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -178,7 +178,6 @@ rcl_client_init( client->impl->options = *options; atomic_init(&client->impl->sequence_number, 0); - client->impl->type_hash = *type_support->get_type_hash_func(type_support); if (RCL_RET_OK != rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), type_support->get_type_description_func(type_support), @@ -189,6 +188,7 @@ rcl_client_init( ret = RCL_RET_ERROR; goto destroy_client; } + client->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); TRACETOOLS_TRACEPOINT( @@ -251,7 +251,10 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) result = RCL_RET_ERROR; } - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &client->impl->type_hash)) { + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != client->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &client->impl->type_hash)) + { RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); result = RCL_RET_ERROR; } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index d5cd4cd12..4e1fee3f0 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -100,8 +100,8 @@ rcl_publisher_init( ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name); // Allocate space for the implementation struct. - publisher->impl = (rcl_publisher_impl_t *)allocator->allocate( - sizeof(rcl_publisher_impl_t), allocator->state); + publisher->impl = (rcl_publisher_impl_t *)allocator->zero_allocate( + 1, sizeof(rcl_publisher_impl_t), allocator->state); RCL_CHECK_FOR_NULL_WITH_MSG( publisher->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); @@ -129,7 +129,6 @@ rcl_publisher_init( // options publisher->impl->options = *options; - publisher->impl->type_hash = *type_support->get_type_hash_func(type_support); if (RCL_RET_OK != rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), type_support->get_type_description_func(type_support), @@ -139,6 +138,7 @@ rcl_publisher_init( RCL_SET_ERROR_MSG("Failed to register type for subscription"); goto fail; } + publisher->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); // context @@ -201,7 +201,10 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) { + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != publisher->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) + { RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); result = RCL_RET_ERROR; } diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 17bd1e1da..1cca57acc 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -189,7 +189,6 @@ rcl_service_init( // options service->impl->options = *options; - service->impl->type_hash = *type_support->get_type_hash_func(type_support); if (RCL_RET_OK != rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), type_support->get_type_description_func(type_support), @@ -200,6 +199,7 @@ rcl_service_init( ret = RCL_RET_ERROR; goto destroy_service; } + service->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); TRACETOOLS_TRACEPOINT( @@ -263,7 +263,10 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) result = RCL_RET_ERROR; } - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) { + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != service->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) + { RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); result = RCL_RET_ERROR; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index b3bb2eb26..1ca6edd8c 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -124,7 +124,6 @@ rcl_subscription_init( // options subscription->impl->options = *options; - subscription->impl->type_hash = *type_support->get_type_hash_func(type_support); if (RCL_RET_OK != rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), type_support->get_type_description_func(type_support), @@ -134,6 +133,7 @@ rcl_subscription_init( RCL_SET_ERROR_MSG("Failed to register type for subscription"); goto fail; } + subscription->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; @@ -206,7 +206,10 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) result = RCL_RET_ERROR; } - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &subscription->impl->type_hash)) { + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != subscription->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &subscription->impl->type_hash)) + { RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); result = RCL_RET_ERROR; diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 87ed39f21..a173200cc 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -64,7 +64,8 @@ _rcl_action_get_zero_initialized_client_impl(void) 0, 0, 0, - 0 + 0, + rosidl_get_zero_initialized_type_hash() }; return null_action_client; } @@ -92,7 +93,10 @@ _rcl_action_client_fini_impl( if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->status_subscription, node)) { ret = RCL_RET_ERROR; } - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_client->impl->type_hash)) { + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != action_client->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_client->impl->type_hash)) + { ret = RCL_RET_ERROR; } allocator.deallocate(action_client->impl->action_name, allocator.state); @@ -226,7 +230,6 @@ rcl_action_client_init( SUBSCRIPTION_INIT(feedback); SUBSCRIPTION_INIT(status); - action_client->impl->type_hash = *type_support->get_type_hash_func(type_support); if (RCL_RET_OK != rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), type_support->get_type_description_func(type_support), @@ -236,7 +239,7 @@ rcl_action_client_init( RCL_SET_ERROR_MSG("Failed to register type for action"); goto fail; } - + action_client->impl->type_hash = *type_support->get_type_hash_func(type_support); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); return ret; diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 84991089f..fbba7b9ec 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -160,6 +160,7 @@ rcl_action_server_init( action_server->impl->goal_handles = NULL; action_server->impl->num_goal_handles = 0u; action_server->impl->clock = NULL; + action_server->impl->type_hash = rosidl_get_zero_initialized_type_hash(); rcl_ret_t ret = RCL_RET_OK; // Initialize services @@ -195,7 +196,6 @@ rcl_action_server_init( } // Store type hash - action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); if (RCL_RET_OK != rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), type_support->get_type_description_func(type_support), @@ -205,6 +205,7 @@ rcl_action_server_init( RCL_SET_ERROR_MSG("Failed to register type for action"); goto fail; } + action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); return ret; fail: @@ -263,7 +264,10 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node) } allocator.deallocate(action_server->impl->goal_handles, allocator.state); action_server->impl->goal_handles = NULL; - if (RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_server->impl->type_hash)) { + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != action_server->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_server->impl->type_hash)) + { ret = RCL_RET_ERROR; } // Deallocate struct From 26d02ad60ba82f6c3ae2c3b4be9cc3efe40d9554 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 5 Jul 2023 12:19:41 -0700 Subject: [PATCH 14/14] remove unneeded (void) Signed-off-by: Emerson Knapp --- rcl/src/rcl/node.c | 1 - 1 file changed, 1 deletion(-) diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 435af671a..b0fe0cfc2 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -561,7 +561,6 @@ void rcl_node_type_description_service_handle_request( const type_description_interfaces__srv__GetTypeDescription_Request * request, type_description_interfaces__srv__GetTypeDescription_Response * response) { - (void)request_header; rcl_type_info_t type_info; RCL_CHECK_FOR_NULL_WITH_MSG(node, "invalid node handle", return;); RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "invalid node", return;);