From eacfc8543ff11ea3f9bf5e30246c6793c5e352d2 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Fri, 31 May 2024 08:49:07 +0200 Subject: [PATCH] micro-ROS Jazzy patch --- .github/workflows/fork_checker.yml | 36 ++++++++++ .vscode/settings.json | 6 ++ rcl/CMakeLists.txt | 69 +++++++++++++++---- rcl/include/rcl/arguments.h | 4 ++ rcl/include/rcl/context.h | 11 ++- rcl/include/rcl/node.h | 2 + rcl/include/rcl/node_options.h | 5 ++ rcl/src/rcl/arguments_impl.h | 4 ++ rcl/src/rcl/client.c | 10 ++- rcl/src/rcl/context.c | 16 ++--- rcl/src/rcl/init.c | 28 +++++--- rcl/src/rcl/logging.c | 10 ++- rcl/src/rcl/node.c | 26 ++++++- rcl/src/rcl/node_impl.h | 2 + rcl/src/rcl/node_options.c | 15 ++++ rcl/src/rcl/node_resolve_name.c | 8 +++ rcl/src/rcl/publisher.c | 7 +- rcl/src/rcl/publisher_impl.h | 2 + rcl/src/rcl/service.c | 11 ++- rcl/src/rcl/subscription.c | 11 ++- rcl/src/rcl/subscription_impl.h | 2 + rcl/src/rcl/type_hash.c | 16 +++++ rcl_action/src/rcl_action/action_client.c | 10 ++- .../src/rcl_action/action_client_impl.h | 2 + rcl_action/src/rcl_action/action_server.c | 11 ++- .../src/rcl_action/action_server_impl.h | 2 + rcl_yaml_param_parser/COLCON_IGNORE | 0 27 files changed, 271 insertions(+), 55 deletions(-) create mode 100644 .github/workflows/fork_checker.yml create mode 100644 .vscode/settings.json create mode 100644 rcl_yaml_param_parser/COLCON_IGNORE diff --git a/.github/workflows/fork_checker.yml b/.github/workflows/fork_checker.yml new file mode 100644 index 000000000..eb5f512c8 --- /dev/null +++ b/.github/workflows/fork_checker.yml @@ -0,0 +1,36 @@ +name: micro-ROS fork Update Checker +on: + workflow_dispatch: + inputs: + name: + description: "Manual trigger" + schedule: + - cron: '0 4 * * *' + +jobs: + micro_ros_fork_update_check: + runs-on: ubuntu-latest + container: ubuntu:20.04 + strategy: + fail-fast: false + matrix: + branches: [galactic, humble, jazzy, rolling] + steps: + - name: Check + id: check + shell: bash + run: | + apt update; apt install -y git + REPO=$(echo ${{ github.repository }} | awk '{split($0,a,"/"); print a[2]}') + git clone -b ${{ matrix.branches }} https://github.com/micro-ros/$REPO + cd $REPO + git remote add ros2 https://github.com/ros2/$REPO + git fetch ros2 + git fetch origin + echo "::set-output name=merge_required::true" + CMP=$(git rev-list --left-right --count ros2/${{ matrix.branches }}...origin/${{ matrix.branches }} | awk '{print $1}') + if [ $CMP = "0" ]; then echo "::set-output name=merge_required::false"; fi + + - name: Alert + if: ${{ steps.check.outputs.merge_required == 'true' }} + run: exit 1 diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 000000000..ce8dc02ec --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "*.uml": "plantuml", + "service_event_publisher.h": "c" + } +} \ No newline at end of file diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index f32797648..e748c51c7 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -2,13 +2,13 @@ cmake_minimum_required(VERSION 3.5) project(rcl) +option(RCL_MICROROS "micro-ROS build mode" ON) + find_package(ament_cmake_ros REQUIRED) find_package(ament_cmake_gen_version_h REQUIRED) -find_package(libyaml_vendor REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcl_logging_interface REQUIRED) -find_package(rcl_yaml_param_parser REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) @@ -16,11 +16,19 @@ find_package(rosidl_runtime_c REQUIRED) find_package(service_msgs REQUIRED) find_package(tracetools REQUIRED) find_package(type_description_interfaces REQUIRED) -find_package(yaml REQUIRED) + +if(NOT RCL_MICROROS) + find_package(libyaml_vendor REQUIRED) + find_package(yaml REQUIRED) + find_package(rcl_yaml_param_parser REQUIRED) +endif() include(cmake/rcl_set_symbol_visibility_hidden.cmake) -include(cmake/get_default_rcl_logging_implementation.cmake) -get_default_rcl_logging_implementation(RCL_LOGGING_IMPL) + +if(NOT RCL_MICROROS) + include(cmake/get_default_rcl_logging_implementation.cmake) + get_default_rcl_logging_implementation(RCL_LOGGING_IMPL) +endif() # Default to C11 if(NOT CMAKE_C_STANDARD) @@ -38,7 +46,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(${PROJECT_NAME}_sources - src/rcl/arguments.c + $<$>:src/rcl/arguments.c> src/rcl/client.c src/rcl/common.c src/rcl/context.c @@ -54,13 +62,13 @@ set(${PROJECT_NAME}_sources src/rcl/lexer.c src/rcl/lexer_lookahead.c src/rcl/localhost.c - src/rcl/logging_rosout.c - src/rcl/logging.c - src/rcl/log_level.c + $<$>:src/rcl/logging_rosout.c> + $<$>:src/rcl/logging.c> + $<$>:src/rcl/log_level.c> src/rcl/network_flow_endpoints.c src/rcl/node.c src/rcl/node_options.c - src/rcl/node_type_cache.c + $<$>:src/rcl/node_type_cache.c> src/rcl/publisher.c src/rcl/remap.c src/rcl/node_resolve_name.c @@ -72,7 +80,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/type_description_conversions.c> src/rcl/validate_enclave_name.c src/rcl/validate_topic_name.c src/rcl/wait.c @@ -88,7 +96,6 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${rcl_interfaces_TARGETS} # TODO(clalancette): rcl_logging_interface should be PRIVATE, but downstream depends on it for now rcl_logging_interface::rcl_logging_interface - rcl_yaml_param_parser::rcl_yaml_param_parser rcutils::rcutils rmw::rmw # TODO(clalancette): rmw_implementation should be PRIVATE, but downstream depends on it for now @@ -96,12 +103,37 @@ target_link_libraries(${PROJECT_NAME} PUBLIC rosidl_runtime_c::rosidl_runtime_c ${type_description_interfaces_TARGETS} ) + +target_link_libraries(${PROJECT_NAME} PRIVATE + tracetools::tracetools +) + +if(NOT RCL_MICROROS) target_link_libraries(${PROJECT_NAME} PRIVATE ${RCL_LOGGING_IMPL}::${RCL_LOGGING_IMPL} ${service_msgs_TARGETS} - tracetools::tracetools yaml ) +endif() + +if(NOT RCL_MICROROS) + ament_target_dependencies(${PROJECT_NAME} + "rcl_yaml_param_parser" + ) +endif() + +if(NOT RCL_MICROROS) + ament_target_dependencies(${PROJECT_NAME} + ${RCL_LOGGING_IMPL} + ) +endif() + +if(NOT RCL_MICROROS) +target_compile_definitions(${PROJECT_NAME} + PUBLIC + RCL_MICROROS_COMPLETE_IMPL + ) +endif() # Allow configuring the default discovery range if(DEFINED RCL_DEFAULT_DISCOVERY_RANGE) @@ -141,14 +173,21 @@ ament_export_dependencies(ament_cmake) ament_export_dependencies(rcl_interfaces) # TODO(clalancette): rcl_logging_interface shouldn't be exported, but downstream depends on it for now ament_export_dependencies(rcl_logging_interface) -ament_export_dependencies(rcl_yaml_param_parser) ament_export_dependencies(rcutils) ament_export_dependencies(rmw) # TODO(clalancette): rmw_implementation shouldn't be exported, but downstream depends on it for now ament_export_dependencies(rmw_implementation) ament_export_dependencies(rosidl_runtime_c) ament_export_dependencies(type_description_interfaces) -ament_generate_version_header(${PROJECT_NAME}) +ament_export_dependencies(tracetools) + +if(NOT RCL_MICROROS) + ament_export_dependencies(rcl_yaml_param_parser) +endif() + +if(NOT RCL_MICROROS) + ament_export_dependencies(${RCL_LOGGING_IMPL}) +endif() if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rcl/include/rcl/arguments.h b/rcl/include/rcl/arguments.h index d37363999..b0289899d 100644 --- a/rcl/include/rcl/arguments.h +++ b/rcl/include/rcl/arguments.h @@ -22,7 +22,11 @@ #include "rcl/macros.h" #include "rcl/types.h" #include "rcl/visibility_control.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl_yaml_param_parser/types.h" +#else +typedef bool rcl_params_t; +#endif // RCL_MICROROS_COMPLETE_IMPL #ifdef __cplusplus extern "C" diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h index 68500da81..06d1ca334 100644 --- a/rcl/include/rcl/context.h +++ b/rcl/include/rcl/context.h @@ -25,7 +25,9 @@ extern "C" #include "rmw/init.h" #include "rcl/allocator.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/arguments.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/init_options.h" #include "rcl/macros.h" #include "rcl/types.h" @@ -112,9 +114,11 @@ typedef struct rcl_context_impl_s rcl_context_impl_t; */ typedef struct rcl_context_s { +#ifdef RCL_MICROROS_COMPLETE_IMPL /// Global arguments for all nodes which share this context. /** Typically generated by the parsing of argc/argv in rcl_init(). */ rcl_arguments_t global_arguments; +#endif // RCL_MICROROS_COMPLETE_IMPL /// Implementation specific pointer. rcl_context_impl_t * impl; @@ -122,11 +126,6 @@ typedef struct rcl_context_s // The assumption that this is big enough for an atomic_uint_least64_t is // ensured with a static_assert in the context.c file. // In most cases it should just be a plain uint64_t. -/// @cond Doxygen_Suppress -#if !defined(RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE) -#define RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE sizeof(uint_least64_t) -#endif -/// @endcond /// Private storage for instance ID atomic. /** * Accessing the instance id should be done using the function @@ -146,7 +145,7 @@ typedef struct rcl_context_s * See this paper for an effort to make this possible in the future: * http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2018/p0943r1.html */ - RCL_ALIGNAS(8) uint8_t instance_id_storage[RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE]; + uint32_t instance_id_storage; } rcl_context_t; /// Return a zero initialization context object. diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 58a2163ec..e58df8d9f 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -25,7 +25,9 @@ extern "C" #include #include "rcl/allocator.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/arguments.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/context.h" #include "rcl/guard_condition.h" #include "rcl/macros.h" diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index 551d79437..a3eae90a6 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -23,7 +23,10 @@ extern "C" #endif #include "rcl/allocator.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/arguments.h" +#endif // RCL_MICROROS_COMPLETE_IMPL +#include "rcl/macros.h" #include "rcl/domain_id.h" @@ -46,8 +49,10 @@ typedef struct rcl_node_options_s /// If false then only use arguments in this struct, otherwise use global arguments also. bool use_global_arguments; +#ifdef RCL_MICROROS_COMPLETE_IMPL /// Command line arguments that apply only to this node. rcl_arguments_t arguments; +#endif // RCL_MICROROS_COMPLETE_IMPL /// Flag to enable rosout for this node bool enable_rosout; diff --git a/rcl/src/rcl/arguments_impl.h b/rcl/src/rcl/arguments_impl.h index e8d73b585..042819802 100644 --- a/rcl/src/rcl/arguments_impl.h +++ b/rcl/src/rcl/arguments_impl.h @@ -17,7 +17,11 @@ #include "rcl/arguments.h" #include "rcl/log_level.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl_yaml_param_parser/types.h" +#else +typedef bool rcl_params_t; +#endif // RCL_MICROROS_COMPLETE_IMPL #include "./remap_impl.h" #ifdef __cplusplus diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 1b8088505..83bfdbebe 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -24,7 +24,9 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/publisher.h" #include "rcl/time.h" #include "rcutils/logging_macros.h" @@ -49,7 +51,9 @@ struct rcl_client_impl_s atomic_int_least64_t sequence_number; rcl_service_event_publisher_t * service_event_publisher; char * remapped_service_name; +#ifdef RCL_MICROROS_COMPLETE_IMPL rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS_COMPLETE_IMPL }; rcl_client_t @@ -178,6 +182,7 @@ rcl_client_init( client->impl->options = *options; atomic_init(&client->impl->sequence_number, 0); +#ifdef RCL_MICROROS_COMPLETE_IMPL const rosidl_type_hash_t * hash = type_support->get_type_hash_func(type_support); if (hash == NULL) { RCL_SET_ERROR_MSG("Failed to get the type hash"); @@ -195,6 +200,7 @@ rcl_client_init( goto destroy_client; } client->impl->type_hash = *hash; +#endif // RCL_MICROROS_COMPLETE_IMPL RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); TRACETOOLS_TRACEPOINT( @@ -256,7 +262,7 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - +#ifdef RCL_MICROROS_COMPLETE_IMPL 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)) @@ -264,7 +270,7 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); result = RCL_RET_ERROR; } - +#endif // RCL_MICROROS_COMPLETE_IMPL allocator.deallocate(client->impl->remapped_service_name, allocator.state); client->impl->remapped_service_name = NULL; diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index 5414cc5e9..495cae549 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -30,16 +30,12 @@ rcl_get_zero_initialized_context(void) { static rcl_context_t context = { .impl = NULL, - .instance_id_storage = {0}, + .instance_id_storage = 0, }; // this is not constexpr so it cannot be in the struct initialization +#ifdef RCL_MICROROS_COMPLETE_IMPL context.global_arguments = rcl_get_zero_initialized_arguments(); - // ensure assumption about static storage - static_assert( - sizeof(context.instance_id_storage) >= sizeof(atomic_uint_least64_t), - "expected rcl_context_t's instance id storage to be >= size of atomic_uint_least64_t"); - // initialize atomic - atomic_init((atomic_uint_least64_t *)(&context.instance_id_storage), 0); +#endif // RCL_MICROROS_COMPLETE_IMPL return context; } @@ -76,7 +72,7 @@ rcl_context_instance_id_t rcl_context_get_instance_id(const rcl_context_t * context) { RCL_CHECK_ARGUMENT_FOR_NULL(context, 0); - return rcutils_atomic_load_uint64_t((atomic_uint_least64_t *)(&context->instance_id_storage)); + return context->instance_id_storage; } rcl_ret_t @@ -110,8 +106,9 @@ __cleanup_context(rcl_context_t * context) { rcl_ret_t ret = RCL_RET_OK; // reset the instance id to 0 to indicate "invalid" (should already be 0, but this is defensive) - rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); + context->instance_id_storage = 0; +#ifdef RCL_MICROROS_COMPLETE_IMPL // clean up global_arguments if initialized if (NULL != context->global_arguments.impl) { ret = rcl_arguments_fini(&(context->global_arguments)); @@ -124,6 +121,7 @@ __cleanup_context(rcl_context_t * context) rcl_reset_error(); } } +#endif // RCL_MICROROS_COMPLETE_IMPL // if impl is null, nothing else can be cleaned up if (NULL != context->impl) { diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index ec05b2d62..068d68187 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -27,22 +27,26 @@ extern "C" #include "tracetools/tracetools.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/arguments.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/discovery_options.h" #include "rcl/domain_id.h" #include "rcl/error_handling.h" #include "rcl/localhost.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/logging.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/security.h" #include "rcl/validate_enclave_name.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "./arguments_impl.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "./common.h" #include "./context_impl.h" #include "./init_options_impl.h" -static atomic_uint_least64_t __rcl_next_unique_id = ATOMIC_VAR_INIT(1); - rcl_ret_t rcl_init( int argc, @@ -81,8 +85,10 @@ rcl_init( return RCL_RET_ALREADY_INIT; } +#ifdef RCL_MICROROS_COMPLETE_IMPL // Zero initialize global arguments. context->global_arguments = rcl_get_zero_initialized_arguments(); +#endif // RCL_MICROROS_COMPLETE_IMPL // Setup impl for context. // use zero_allocate so the cleanup function will not try to clean up uninitialized parts later @@ -124,6 +130,7 @@ rcl_init( } } +#ifdef RCL_MICROROS_COMPLETE_IMPL // Parse the ROS specific arguments. ret = rcl_parse_arguments(argc, argv, allocator, &context->global_arguments); if (RCL_RET_OK != ret) { @@ -131,17 +138,16 @@ rcl_init( RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to parse global arguments"); goto fail; } +#endif // RCL_MICROROS_COMPLETE_IMPL // Set the instance id. - uint64_t next_instance_id = rcutils_atomic_fetch_add_uint64_t(&__rcl_next_unique_id, 1); + static uint32_t next_instance_id = 0; + next_instance_id++; if (0 == next_instance_id) { - // Roll over occurred, this is an extremely unlikely occurrence. - RCL_SET_ERROR_MSG("unique rcl instance ids exhausted"); - // Roll back to try to avoid the next call succeeding, but there's a data race here. - rcutils_atomic_store(&__rcl_next_unique_id, -1); - goto fail; + // Avoid invalid value on roll over + next_instance_id++; } - rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), next_instance_id); + context->instance_id_storage = next_instance_id; context->impl->init_options.impl->rmw_init_options.instance_id = next_instance_id; size_t * domain_id = &context->impl->init_options.impl->rmw_init_options.domain_id; @@ -256,6 +262,7 @@ rcl_init( "\t%s", discovery_options->static_peers[ii].peer_address); } +#ifdef RCL_MICROROS_COMPLETE_IMPL if (context->global_arguments.impl->enclave) { context->impl->init_options.impl->rmw_init_options.enclave = rcutils_strdup( context->global_arguments.impl->enclave, @@ -270,6 +277,7 @@ rcl_init( fail_ret = RCL_RET_BAD_ALLOC; goto fail; } +#endif //RCL_MICROROS int validation_result; size_t invalid_index; @@ -341,7 +349,7 @@ rcl_shutdown(rcl_context_t * context) } // reset the instance id to 0 to indicate "invalid" - rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); + context->instance_id_storage = 0; return RCL_RET_OK; } diff --git a/rcl/src/rcl/logging.c b/rcl/src/rcl/logging.c index 636a90d50..0fe882402 100644 --- a/rcl/src/rcl/logging.c +++ b/rcl/src/rcl/logging.c @@ -68,9 +68,13 @@ rcl_logging_configure_with_output_handler( rcl_log_levels_t * log_levels = &global_args->impl->log_levels; const char * file_name_prefix = global_args->impl->external_log_file_name_prefix; const char * config_file = global_args->impl->external_log_config_file; - g_rcl_logging_stdout_enabled = !global_args->impl->log_stdout_disabled; - g_rcl_logging_rosout_enabled = !global_args->impl->log_rosout_disabled; - g_rcl_logging_ext_lib_enabled = !global_args->impl->log_ext_lib_disabled; + bool global_disable = false; + #ifdef RCUTILS_NO_LOGGING + global_disable = true; + #endif + g_rcl_logging_stdout_enabled = !global_args->impl->log_stdout_disabled && !global_disable; + g_rcl_logging_rosout_enabled = !global_args->impl->log_rosout_disabled && !global_disable; + g_rcl_logging_ext_lib_enabled = !global_args->impl->log_ext_lib_disabled && !global_disable; rcl_ret_t status = RCL_RET_OK; g_rcl_logging_num_out_handlers = 0; diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index 998ce1ab6..0a1c6c666 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -19,13 +19,17 @@ #include #include +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/arguments.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/error_handling.h" #include "rcl/init_options.h" #include "rcl/localhost.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/logging.h" #include "rcl/logging_rosout.h" #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/rcl.h" #include "rcl/remap.h" #include "rcl/security.h" @@ -193,7 +197,9 @@ rcl_node_init( RCL_CHECK_FOR_NULL_WITH_MSG( node->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto fail); node->impl->options = rcl_node_get_default_options(); +#ifdef RCL_MICROROS_COMPLETE_IMPL node->impl->registered_types_by_type_hash = rcutils_get_zero_initialized_hash_map(); +#endif // RCL_MICROROS_COMPLETE_IMPL node->context = context; // Initialize node impl. ret = rcl_node_options_copy(options, &(node->impl->options)); @@ -201,6 +207,7 @@ rcl_node_init( goto fail; } +#ifdef RCL_MICROROS_COMPLETE_IMPL // Remap the node name and namespace if remap rules are given rcl_arguments_t * global_args = NULL; if (node->impl->options.use_global_arguments) { @@ -227,6 +234,7 @@ rcl_node_init( allocator->deallocate((char *)local_namespace_, allocator->state); local_namespace_ = remapped_namespace; } +#endif // RCL_MICROROS_COMPLETE_IMPL // compute fully qualfied name of the node. if ('/' == local_namespace_[strlen(local_namespace_) - 1]) { @@ -239,9 +247,11 @@ rcl_node_init( ret = RCL_RET_BAD_ALLOC; goto fail); // node logger name +#ifdef RCL_MICROROS_COMPLETE_IMPL node->impl->logger_name = rcl_create_node_logger_name(name, local_namespace_, allocator); RCL_CHECK_FOR_NULL_WITH_MSG( node->impl->logger_name, "creating logger name failed", ret = RCL_RET_ERROR; goto fail); +#endif // RCL_MICROROS_COMPLETE_IMPL RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Using domain ID of '%zu'", context->impl->rmw_context.actual_domain_id); @@ -274,12 +284,14 @@ rcl_node_init( goto fail; } +#ifdef RCL_MICROROS_COMPLETE_IMPL // 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; } +#endif // RCL_MICROROS_COMPLETE_IMPL RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); TRACETOOLS_TRACEPOINT( @@ -296,12 +308,14 @@ rcl_node_init( fail: if (node->impl) { +#ifdef RCL_MICROROS_COMPLETE_IMPL if (NULL != node->impl->registered_types_by_type_hash.impl) { fail_ret = rcl_node_type_cache_fini(node); RCUTILS_LOG_ERROR_EXPRESSION_NAMED( (fail_ret != RCL_RET_OK), ROS_PACKAGE_NAME, "Failed to fini type cache for node: %s", rcl_get_error_string().str); } +#endif // RCL_MICROROS_COMPLETE_IMPL if (node->impl->graph_guard_condition) { fail_ret = rcl_guard_condition_fini(node->impl->graph_guard_condition); @@ -352,11 +366,13 @@ rcl_node_fini(rcl_node_t * node) rcl_allocator_t allocator = node->impl->options.allocator; rcl_ret_t result = RCL_RET_OK; rcl_ret_t rcl_ret = RCL_RET_OK; +#ifdef RCL_MICROROS_COMPLETE_IMPL rcl_ret = rcl_node_type_cache_fini(node); if (rcl_ret != RCL_RET_OK) { RCL_SET_ERROR_MSG("Unable to fini type cache for node."); result = RCL_RET_ERROR; } +#endif // RCL_MICROROS_COMPLETE_IMPL 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); @@ -371,12 +387,14 @@ rcl_node_fini(rcl_node_t * node) // assuming that allocate and deallocate are ok since they are checked in init allocator.deallocate((char *)node->impl->logger_name, allocator.state); allocator.deallocate((char *)node->impl->fq_name, allocator.state); +#ifdef RCL_MICROROS_COMPLETE_IMPL if (NULL != node->impl->options.arguments.impl) { rcl_ret_t ret = rcl_arguments_fini(&(node->impl->options.arguments)); if (ret != RCL_RET_OK) { return ret; } } +#endif // RCL_MICROROS_COMPLETE_IMPL allocator.deallocate(node->impl, allocator.state); node->impl = NULL; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized"); @@ -519,6 +537,7 @@ void rcl_node_type_description_service_handle_request( const type_description_interfaces__srv__GetTypeDescription_Request * request, type_description_interfaces__srv__GetTypeDescription_Response * response) { +#ifdef RCL_MICROROS_COMPLETE_IMPL 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;); @@ -577,12 +596,14 @@ void rcl_node_type_description_service_handle_request( } response->successful = true; +#endif //RCL_MICROROS } rcl_ret_t rcl_node_type_description_service_init( rcl_service_t * service, const rcl_node_t * node) { +#ifdef RCL_MICROROS_COMPLETE_IMPL RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); @@ -615,6 +636,9 @@ rcl_ret_t rcl_node_type_description_service_init( service, node, type_support, service_name, &service_ops); allocator.deallocate(service_name, allocator.state); - return ret; +#else + (void)node; + return RCL_RET_UNSUPPORTED; +#endif //RCL_MICROROS } diff --git a/rcl/src/rcl/node_impl.h b/rcl/src/rcl/node_impl.h index 5445c5aab..42818042e 100644 --- a/rcl/src/rcl/node_impl.h +++ b/rcl/src/rcl/node_impl.h @@ -30,7 +30,9 @@ struct rcl_node_impl_s rcl_guard_condition_t * graph_guard_condition; const char * logger_name; const char * fq_name; +#ifdef RCL_MICROROS_COMPLETE_IMPL rcutils_hash_map_t registered_types_by_type_hash; +#endif // RCL_MICROROS_COMPLETE_IMPL }; #endif // RCL__NODE_IMPL_H_ diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index f2a50df62..9f0a875ff 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -21,10 +21,14 @@ extern "C" #include "rcl/node_options.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/arguments.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/domain_id.h" #include "rcl/error_handling.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/logging_rosout.h" +#endif // RCL_MICROROS_COMPLETE_IMPL rcl_node_options_t rcl_node_get_default_options(void) @@ -33,9 +37,13 @@ rcl_node_get_default_options(void) rcl_node_options_t default_options = { .allocator = rcl_get_default_allocator(), .use_global_arguments = true, + #ifdef RCL_MICROROS_COMPLETE_IMPL .arguments = rcl_get_zero_initialized_arguments(), .enable_rosout = true, .rosout_qos = rcl_qos_profile_rosout_default, + #else + .enable_rosout = false, + #endif // RCL_MICROROS_COMPLETE_IMPL }; return default_options; } @@ -53,17 +61,22 @@ rcl_node_options_copy( RCL_SET_ERROR_MSG("Attempted to copy options into itself"); return RCL_RET_INVALID_ARGUMENT; } +#ifdef RCL_MICROROS_COMPLETE_IMPL if (NULL != options_out->arguments.impl) { RCL_SET_ERROR_MSG("Options out must be zero initialized"); return RCL_RET_INVALID_ARGUMENT; } +#endif // RCL_MICROROS_COMPLETE_IMPL + options_out->allocator = options->allocator; options_out->use_global_arguments = options->use_global_arguments; options_out->enable_rosout = options->enable_rosout; options_out->rosout_qos = options->rosout_qos; +#ifdef RCL_MICROROS_COMPLETE_IMPL if (NULL != options->arguments.impl) { return rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); } +#endif // RCL_MICROROS_COMPLETE_IMPL return RCL_RET_OK; } @@ -75,6 +88,7 @@ rcl_node_options_fini( rcl_allocator_t allocator = options->allocator; RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); +#ifdef RCL_MICROROS_COMPLETE_IMPL if (options->arguments.impl) { rcl_ret_t ret = rcl_arguments_fini(&options->arguments); if (RCL_RET_OK != ret) { @@ -82,6 +96,7 @@ rcl_node_options_fini( return ret; } } +#endif // RCL_MICROROS_COMPLETE_IMPL return RCL_RET_OK; } diff --git a/rcl/src/rcl/node_resolve_name.c b/rcl/src/rcl/node_resolve_name.c index 9c2667669..ce71349c5 100644 --- a/rcl/src/rcl/node_resolve_name.c +++ b/rcl/src/rcl/node_resolve_name.c @@ -75,6 +75,7 @@ rcl_resolve_name( goto cleanup; } // remap topic name +#ifdef RCL_MICROROS_COMPLETE_IMPL if (!only_expand) { ret = rcl_remap_name( local_args, global_args, is_service ? RCL_SERVICE_REMAP : RCL_TOPIC_REMAP, @@ -84,6 +85,7 @@ rcl_resolve_name( goto cleanup; } } +#endif // RCL_MICROROS_COMPLETE_IMPL if (NULL == remapped_topic_name) { remapped_topic_name = expanded_topic_name; expanded_topic_name = NULL; @@ -145,12 +147,18 @@ rcl_node_resolve_name( return RCL_RET_ERROR; } rcl_arguments_t * global_args = NULL; +#ifdef RCL_MICROROS_COMPLETE_IMPL if (node_options->use_global_arguments) { global_args = &(node->context->global_arguments); } +#endif // RCL_MICROROS_COMPLETE_IMPL return rcl_resolve_name( +#ifdef RCL_MICROROS_COMPLETE_IMPL &(node_options->arguments), +#else + NULL, +#endif // RCL_MICROROS_COMPLETE_IMPL global_args, input_topic_name, rcl_node_get_name(node), diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 60963254b..1a76bddf1 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -25,7 +25,9 @@ extern "C" #include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rcl/node.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rcl/time.h" @@ -128,7 +130,7 @@ rcl_publisher_init( options->qos.avoid_ros_namespace_conventions; // options publisher->impl->options = *options; - +#ifdef RCL_MICROROS_COMPLETE_IMPL 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 +141,7 @@ rcl_publisher_init( goto fail; } publisher->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS_COMPLETE_IMPL RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); // context @@ -201,6 +204,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } +#ifdef RCL_MICROROS_COMPLETE_IMPL 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)) @@ -208,6 +212,7 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); result = RCL_RET_ERROR; } +#endif // RCL_MICROROS_COMPLETE_IMPL 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 26bbb43da..d9edd4316 100644 --- a/rcl/src/rcl/publisher_impl.h +++ b/rcl/src/rcl/publisher_impl.h @@ -25,7 +25,9 @@ struct rcl_publisher_impl_s rmw_qos_profile_t actual_qos; rcl_context_t * context; rmw_publisher_t * rmw_handle; +#ifdef RCL_MICROROS_COMPLETE_IMPL rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS_COMPLETE_IMPL }; #endif // RCL__PUBLISHER_IMPL_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 660fc8daf..f8c5b533d 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -24,7 +24,9 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/publisher.h" #include "rcl/time.h" #include "rcl/types.h" @@ -48,7 +50,9 @@ struct rcl_service_impl_s rmw_service_t * rmw_handle; rcl_service_event_publisher_t * service_event_publisher; char * remapped_service_name; +#ifdef RCL_MICROROS_COMPLETE_IMPL rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS_COMPLETE_IMPL }; rcl_service_t @@ -188,7 +192,7 @@ rcl_service_init( // options service->impl->options = *options; - +#ifdef RCL_MICROROS_COMPLETE_IMPL 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 +204,7 @@ rcl_service_init( goto destroy_service; } service->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS_COMPLETE_IMPL RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); TRACETOOLS_TRACEPOINT( @@ -262,7 +267,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - +#ifdef RCL_MICROROS_COMPLETE_IMPL 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)) @@ -270,7 +275,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); result = RCL_RET_ERROR; } - +#endif // RCL_MICROROS_COMPLETE_IMPL 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 8d4ea54aa..1edbdbfa5 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -23,8 +23,10 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/node_type_cache.h" #include "rcutils/env.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" #include "rcutils/types/string_array.h" @@ -124,7 +126,7 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; // options subscription->impl->options = *options; - +#ifdef RCL_MICROROS_COMPLETE_IMPL 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), @@ -135,6 +137,7 @@ rcl_subscription_init( goto fail; } subscription->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS_COMPLETE_IMPL RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; @@ -206,7 +209,7 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); result = RCL_RET_ERROR; } - +#ifdef RCL_MICROROS_COMPLETE_IMPL 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)) @@ -215,7 +218,7 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); result = RCL_RET_ERROR; } - +#endif // RCL_MICROROS_COMPLETE_IMPL allocator.deallocate(subscription->impl, allocator.state); subscription->impl = NULL; } @@ -233,6 +236,7 @@ rcl_subscription_get_default_options(void) default_options.allocator = rcl_get_default_allocator(); default_options.rmw_subscription_options = rmw_get_default_subscription_options(); +#ifdef RCL_MICROROS_COMPLETE_IMPL // Load disable flag to LoanedMessage via environmental variable. // TODO(clalancette): This is kind of a copy of rcl_get_disable_loaned_message(), but we need // more information than that function provides. @@ -248,6 +252,7 @@ rcl_subscription_get_default_options(void) } else { default_options.disable_loaned_message = !(strcmp(env_val, "0") == 0); } +#endif // RCL_MICROROS_COMPLETE_IMPL return default_options; } diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 8193421cf..64c0b75a1 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -24,7 +24,9 @@ struct rcl_subscription_impl_s rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; +#ifdef RCL_MICROROS_COMPLETE_IMPL rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS_COMPLETE_IMPL }; #endif // RCL__SUBSCRIPTION_IMPL_H_ diff --git a/rcl/src/rcl/type_hash.c b/rcl/src/rcl/type_hash.c index f05dd9851..f70809c92 100644 --- a/rcl/src/rcl/type_hash.c +++ b/rcl/src/rcl/type_hash.c @@ -14,7 +14,9 @@ #include +#ifdef RCL_MICROROS_COMPLETE_IMPL #include +#endif // RCL_MICROROS_COMPLETE_IMPL #include "rcl/allocator.h" #include "rcl/error_handling.h" @@ -23,6 +25,7 @@ #include "rcutils/sha256.h" #include "type_description_interfaces/msg/type_description.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL static int yaml_write_handler(void * ext, uint8_t * buffer, size_t size) { rcutils_char_array_t * repr = (rcutils_char_array_t *)ext; @@ -191,12 +194,14 @@ static int emit_type_description( } return end_sequence(emitter) && end_mapping(emitter); } +#endif // RCL_MICROROS_COMPLETE_IMPL rcl_ret_t rcl_type_description_to_hashable_json( const type_description_interfaces__msg__TypeDescription * type_description, rcutils_char_array_t * output_repr) { +#ifdef RCL_MICROROS_COMPLETE_IMPL RCL_CHECK_ARGUMENT_FOR_NULL(type_description, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(output_repr, RCL_RET_INVALID_ARGUMENT); @@ -238,6 +243,11 @@ rcl_type_description_to_hashable_json( rcl_set_error_state(emitter.problem, __FILE__, __LINE__); yaml_emitter_delete(&emitter); return RCL_RET_ERROR; +#else + (void)type_description; + (void)output_repr; + return RCL_RET_UNSUPPORTED; +#endif // RCL_MICROROS_COMPLETE_IMPL } rcl_ret_t @@ -245,6 +255,7 @@ rcl_calculate_type_hash( const type_description_interfaces__msg__TypeDescription * type_description, rosidl_type_hash_t * output_hash) { +#ifdef RCL_MICROROS_COMPLETE_IMPL RCL_CHECK_ARGUMENT_FOR_NULL(type_description, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(output_hash, RCL_RET_INVALID_ARGUMENT); @@ -263,4 +274,9 @@ rcl_calculate_type_hash( } result = rcutils_char_array_fini(&msg_repr); return result; +#else + (void)type_description; + (void)output_hash; + return RCL_RET_UNSUPPORTED; +#endif // RCL_MICROROS_COMPLETE_IMPL } diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index a173200cc..bc6d779da 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -25,10 +25,12 @@ extern "C" #include "rcl_action/types.h" #include "rcl_action/wait.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL +#include "rcl/node_type_cache.h" +#endif // RCL_MICROROS_COMPLETE_IMPL #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" @@ -65,7 +67,9 @@ _rcl_action_get_zero_initialized_client_impl(void) 0, 0, 0, +#ifdef RCL_MICROROS_COMPLETE_IMPL rosidl_get_zero_initialized_type_hash() +#endif // RCL_MICROROS_COMPLETE_IMPL }; return null_action_client; } @@ -93,12 +97,14 @@ _rcl_action_client_fini_impl( if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->status_subscription, node)) { ret = RCL_RET_ERROR; } +#ifdef RCL_MICROROS_COMPLETE_IMPL 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; } +#endif // RCL_MICROROS_COMPLETE_IMPL allocator.deallocate(action_client->impl->action_name, allocator.state); allocator.deallocate(action_client->impl, allocator.state); action_client->impl = NULL; @@ -230,6 +236,7 @@ rcl_action_client_init( SUBSCRIPTION_INIT(feedback); SUBSCRIPTION_INIT(status); +#ifdef RCL_MICROROS_COMPLETE_IMPL 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), @@ -240,6 +247,7 @@ rcl_action_client_init( goto fail; } action_client->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS_COMPLETE_IMPL RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); return ret; diff --git a/rcl_action/src/rcl_action/action_client_impl.h b/rcl_action/src/rcl_action/action_client_impl.h index f1d430a8d..ad1098609 100644 --- a/rcl_action/src/rcl_action/action_client_impl.h +++ b/rcl_action/src/rcl_action/action_client_impl.h @@ -33,7 +33,9 @@ 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; +#ifdef RCL_MICROROS_COMPLETE_IMPL rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS_COMPLETE_IMPL } 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 db48096ed..d75d24af7 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -26,8 +26,10 @@ extern "C" #include "rcl_action/types.h" #include "rcl_action/wait.h" -#include "rcl/error_handling.h" +#ifdef RCL_MICROROS_COMPLETE_IMPL #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS_COMPLETE_IMPL +#include "rcl/error_handling.h" #include "rcl/rcl.h" #include "rcl/time.h" @@ -160,7 +162,9 @@ rcl_action_server_init( action_server->impl->goal_handles = NULL; action_server->impl->num_goal_handles = 0u; action_server->impl->clock = NULL; +#ifdef RCL_MICROROS_COMPLETE_IMPL action_server->impl->type_hash = rosidl_get_zero_initialized_type_hash(); +#endif // RCL_MICROROS_COMPLETE_IMPL rcl_ret_t ret = RCL_RET_OK; // Initialize services @@ -195,6 +199,7 @@ rcl_action_server_init( goto fail; } +#ifdef RCL_MICROROS_COMPLETE_IMPL // Store type hash if (RCL_RET_OK != rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), @@ -206,6 +211,7 @@ rcl_action_server_init( goto fail; } action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS_COMPLETE_IMPL return ret; fail: @@ -264,12 +270,15 @@ 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; + +#ifdef RCL_MICROROS_COMPLETE_IMPL 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; } +#endif // RCL_MICROROS_COMPLETE_IMPL // 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 572f799f6..f3eaed24c 100644 --- a/rcl_action/src/rcl_action/action_server_impl.h +++ b/rcl_action/src/rcl_action/action_server_impl.h @@ -39,7 +39,9 @@ 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; +#ifdef RCL_MICROROS_COMPLETE_IMPL rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS_COMPLETE_IMPL } rcl_action_server_impl_t; diff --git a/rcl_yaml_param_parser/COLCON_IGNORE b/rcl_yaml_param_parser/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb