diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index b61cd06f5..43657cdef 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -2,13 +2,13 @@ cmake_minimum_required(VERSION 3.20) project(rcl) +option(RCL_MICROROS "micro-ROS build mode" OFF) + 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,14 +16,20 @@ 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(rcl_yaml_param_parser REQUIRED) + find_package(yaml REQUIRED) +endif() include(cmake/rcl_set_symbol_visibility_hidden.cmake) -# Determine the logging implementation to use -include(cmake/get_default_rcl_logging_implementation.cmake) -get_default_rcl_logging_implementation(RCL_LOGGING_IMPL) -message(STATUS "RCL_LOGGING_IMPLEMENTATION: ${RCL_LOGGING_IMPL}") +if(NOT RCL_MICROROS) + # Determine the logging implementation to use + include(cmake/get_default_rcl_logging_implementation.cmake) + get_default_rcl_logging_implementation(RCL_LOGGING_IMPL) + message(STATUS "RCL_LOGGING_IMPLEMENTATION: ${RCL_LOGGING_IMPL}") +endif() # Default to C11 if(NOT CMAKE_C_STANDARD) @@ -41,7 +47,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 @@ -57,13 +63,13 @@ set(${PROJECT_NAME}_sources src/rcl/init_options.c src/rcl/lexer.c src/rcl/lexer_lookahead.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 @@ -75,13 +81,16 @@ 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 ) add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources}) +if(RCL_MICROROS) + target_compile_definitions(${PROJECT_NAME} PUBLIC RCL_MICROROS) +endif() target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" @@ -91,7 +100,6 @@ target_link_libraries(${PROJECT_NAME} PUBLIC rcl_interfaces::rcl_interfaces # 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 @@ -99,25 +107,37 @@ target_link_libraries(${PROJECT_NAME} PUBLIC rosidl_runtime_c::rosidl_runtime_c type_description_interfaces::type_description_interfaces ) +if(NOT RCL_MICROROS) + target_link_libraries(${PROJECT_NAME} PUBLIC + rcl_yaml_param_parser::rcl_yaml_param_parser + ) +endif() + target_link_libraries(${PROJECT_NAME} PRIVATE - service_msgs::service_msgs tracetools::tracetools - yaml ) - -# Link logging implementation based on configuration -if(RCL_LOGGING_IMPL STREQUAL "rcl_logging_implementation") - # Dynamic loading: use rcl_logging_implementation abstraction - target_link_libraries(${PROJECT_NAME} PRIVATE - rcl_logging_implementation::rcl_logging_implementation - ) -else() - # Static linking: use specific logging implementation +if(NOT RCL_MICROROS) target_link_libraries(${PROJECT_NAME} PRIVATE - ${RCL_LOGGING_IMPL}::${RCL_LOGGING_IMPL} + service_msgs::service_msgs + yaml ) endif() +if(NOT RCL_MICROROS) + # Link logging implementation based on configuration + if(RCL_LOGGING_IMPL STREQUAL "rcl_logging_implementation") + # Dynamic loading: use rcl_logging_implementation abstraction + target_link_libraries(${PROJECT_NAME} PRIVATE + rcl_logging_implementation::rcl_logging_implementation + ) + else() + # Static linking: use specific logging implementation + target_link_libraries(${PROJECT_NAME} PRIVATE + ${RCL_LOGGING_IMPL}::${RCL_LOGGING_IMPL} + ) + endif() +endif() + # Allow configuring the default discovery range if(DEFINED RCL_DEFAULT_DISCOVERY_RANGE) target_compile_definitions(${PROJECT_NAME} PRIVATE @@ -156,13 +176,16 @@ 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(tracetools) ament_export_dependencies(type_description_interfaces) +if(NOT RCL_MICROROS) + ament_export_dependencies(rcl_yaml_param_parser) +endif() ament_generate_version_header(${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/rcl/include/rcl/arguments.h b/rcl/include/rcl/arguments.h index d37363999..d1371cb5d 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" +#ifndef RCL_MICROROS #include "rcl_yaml_param_parser/types.h" +#else +typedef bool rcl_params_t; +#endif // RCL_MICROROS #ifdef __cplusplus extern "C" diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h index 68500da81..23fbbfe5c 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" +#ifndef RCL_MICROROS #include "rcl/arguments.h" +#endif // RCL_MICROROS #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 { +#ifndef RCL_MICROROS /// 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 /// Implementation specific pointer. rcl_context_impl_t * impl; @@ -123,7 +127,7 @@ typedef struct rcl_context_s // 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) +#if !defined(RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE) && !defined(RCL_MICROROS) #define RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE sizeof(uint_least64_t) #endif /// @endcond @@ -146,7 +150,11 @@ 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 */ +#ifdef RCL_MICROROS + uint32_t instance_id_storage; +#else RCL_ALIGNAS(8) uint8_t instance_id_storage[RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE]; +#endif // RCL_MICROROS } 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..cf3193090 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -25,7 +25,9 @@ extern "C" #include #include "rcl/allocator.h" +#ifndef RCL_MICROROS #include "rcl/arguments.h" +#endif // RCL_MICROROS #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 140749f49..8380776be 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -23,9 +23,11 @@ extern "C" #endif #include "rcl/allocator.h" +#ifndef RCL_MICROROS #include "rcl/arguments.h" - +#endif // RCL_MICROROS #include "rcl/domain_id.h" +#include "rcl/macros.h" /// Constant which indicates that the default domain id should be used. #define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID RCL_DEFAULT_DOMAIN_ID @@ -46,8 +48,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; +#ifndef RCL_MICROROS /// Command line arguments that apply only to this node. rcl_arguments_t arguments; +#endif // RCL_MICROROS /// 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..0cc965a06 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" +#ifndef RCL_MICROROS #include "rcl_yaml_param_parser/types.h" +#else +typedef bool rcl_params_t; +#endif // RCL_MICROROS #include "./remap_impl.h" #ifdef __cplusplus diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 7529c783d..7b632ccf0 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" +#ifndef RCL_MICROROS #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS #include "rcl/publisher.h" #include "rcl/time.h" #include "rcutils/logging_macros.h" @@ -169,6 +171,7 @@ rcl_client_init( atomic_init(&client->impl->sequence_number, 0); client->impl->in_use_by_waitset = false; +#ifndef RCL_MICROROS 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"); @@ -186,6 +189,7 @@ rcl_client_init( goto destroy_client; } client->impl->type_hash = *hash; +#endif // RCL_MICROROS RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); TRACETOOLS_TRACEPOINT( @@ -248,6 +252,7 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) result = RCL_RET_ERROR; } +#ifndef RCL_MICROROS 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)) @@ -255,6 +260,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 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 7f7d37144..f372bcbca 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -32,6 +32,7 @@ rcl_get_zero_initialized_context(void) .impl = NULL, .instance_id_storage = {0}, }; +#ifndef RCL_MICROROS // this is not constexpr so it cannot be in the struct initialization context.global_arguments = rcl_get_zero_initialized_arguments(); // ensure assumption about static storage @@ -40,6 +41,7 @@ rcl_get_zero_initialized_context(void) "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 return context; } @@ -76,7 +78,11 @@ rcl_context_instance_id_t rcl_context_get_instance_id(const rcl_context_t * context) { RCL_CHECK_ARGUMENT_FOR_NULL(context, 0); +#ifdef RCL_MICROROS + return context->instance_id_storage; +#else return rcutils_atomic_load_uint64_t((atomic_uint_least64_t *)(&context->instance_id_storage)); +#endif // RCL_MICROROS } rcl_ret_t @@ -110,8 +116,13 @@ __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) +#ifdef RCL_MICROROS + context->instance_id_storage = 0; +#else rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); +#endif // RCL_MICROROS +#ifndef RCL_MICROROS // clean up global_arguments if initialized if (NULL != context->global_arguments.impl) { ret = rcl_arguments_fini(&(context->global_arguments)); @@ -124,6 +135,7 @@ __cleanup_context(rcl_context_t * context) rcl_reset_error(); } } +#endif // RCL_MICROROS // 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 ef54321a4..cd92fcef6 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -27,20 +27,28 @@ extern "C" #include "tracetools/tracetools.h" +#ifndef RCL_MICROROS #include "rcl/arguments.h" +#endif // RCL_MICROROS #include "rcl/discovery_options.h" #include "rcl/domain_id.h" #include "rcl/error_handling.h" +#ifndef RCL_MICROROS #include "rcl/logging.h" +#endif // RCL_MICROROS #include "rcl/security.h" #include "rcl/validate_enclave_name.h" +#ifndef RCL_MICROROS #include "./arguments_impl.h" +#endif // RCL_MICROROS #include "./common.h" #include "./context_impl.h" #include "./init_options_impl.h" +#ifndef RCL_MICROROS static atomic_uint_least64_t __rcl_next_unique_id = ATOMIC_VAR_INIT(1); +#endif // RCL_MICROROS rcl_ret_t rcl_init( @@ -81,8 +89,10 @@ rcl_init( return RCL_RET_ALREADY_INIT; } +#ifndef RCL_MICROROS // Zero initialize global arguments. context->global_arguments = rcl_get_zero_initialized_arguments(); +#endif // RCL_MICROROS // Setup impl for context. // use zero_allocate so the cleanup function will not try to clean up uninitialized parts later @@ -124,6 +134,7 @@ rcl_init( } } +#ifndef RCL_MICROROS // Parse the ROS specific arguments. ret = rcl_parse_arguments(argc, argv, allocator, &context->global_arguments); if (RCL_RET_OK != ret) { @@ -131,7 +142,9 @@ rcl_init( RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to parse global arguments"); goto fail; } +#endif // RCL_MICROROS +#ifndef RCL_MICROROS // Set the instance id. uint64_t next_instance_id = rcutils_atomic_fetch_add_uint64_t(&__rcl_next_unique_id, 1); if (0 == next_instance_id) { @@ -141,7 +154,13 @@ rcl_init( rcutils_atomic_store(&__rcl_next_unique_id, -1); goto fail; } +#endif // RCL_MICROROS +#ifdef RCL_MICROROS + static uint32_t next_instance_id = 1; + context->instance_id_storage = next_instance_id; +#else rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), next_instance_id); +#endif // RCL_MICROROS 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; @@ -214,6 +233,7 @@ rcl_init( "\t%s", discovery_options->static_peers[ii].peer_address); } +#ifndef RCL_MICROROS if (context->global_arguments.impl->enclave) { context->impl->init_options.impl->rmw_init_options.enclave = rcutils_strdup( context->global_arguments.impl->enclave, @@ -259,6 +279,7 @@ rcl_init( fail_ret = ret; goto fail; } +#endif // RCL_MICROROS // Initialize rmw_init. rmw_ret_t rmw_ret = rmw_init( @@ -299,7 +320,11 @@ rcl_shutdown(rcl_context_t * context) } // reset the instance id to 0 to indicate "invalid" +#ifdef RCL_MICROROS + context->instance_id_storage = 0; +#else rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); +#endif // RCL_MICROROS return RCL_RET_OK; } diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index c1d286a26..f22d22338 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -19,12 +19,16 @@ #include #include +#ifndef RCL_MICROROS #include "rcl/arguments.h" +#endif // RCL_MICROROS #include "rcl/error_handling.h" #include "rcl/init_options.h" +#ifndef RCL_MICROROS #include "rcl/logging.h" #include "rcl/logging_rosout.h" #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS #include "rcl/rcl.h" #include "rcl/remap.h" #include "rcl/security.h" @@ -190,7 +194,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(); +#ifndef RCL_MICROROS node->impl->registered_types_by_type_hash = rcutils_get_zero_initialized_hash_map(); +#endif // RCL_MICROROS node->context = context; // Initialize node impl. ret = rcl_node_options_copy(options, &(node->impl->options)); @@ -198,6 +204,7 @@ rcl_node_init( goto fail; } +#ifndef RCL_MICROROS // Remap the node name and namespace if remap rules are given rcl_arguments_t * global_args = NULL; if (node->impl->options.use_global_arguments) { @@ -224,6 +231,7 @@ rcl_node_init( allocator->deallocate((char *)local_namespace_, allocator->state); local_namespace_ = remapped_namespace; } +#endif // RCL_MICROROS // compute fully qualfied name of the node. if ('/' == local_namespace_[strlen(local_namespace_) - 1]) { @@ -236,9 +244,11 @@ rcl_node_init( ret = RCL_RET_BAD_ALLOC; goto fail); // node logger name +#ifndef RCL_MICROROS 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 RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Using domain ID of '%zu'", context->impl->rmw_context.actual_domain_id); @@ -279,12 +289,14 @@ rcl_node_init( goto fail; } +#ifndef RCL_MICROROS // 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 RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); TRACETOOLS_TRACEPOINT( @@ -301,12 +313,14 @@ rcl_node_init( fail: if (node->impl) { +#ifndef RCL_MICROROS 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 if (node->impl->graph_guard_condition) { fail_ret = rcl_guard_condition_fini(node->impl->graph_guard_condition); @@ -357,11 +371,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; +#ifndef RCL_MICROROS 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 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); @@ -376,12 +392,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); +#ifndef RCL_MICROROS 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 allocator.deallocate(node->impl, allocator.state); node->impl = NULL; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized"); @@ -524,6 +542,7 @@ void rcl_node_type_description_service_handle_request( const type_description_interfaces__srv__GetTypeDescription_Request * request, type_description_interfaces__srv__GetTypeDescription_Response * response) { +#ifndef RCL_MICROROS 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;); @@ -582,12 +601,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) { +#ifndef RCL_MICROROS 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); @@ -622,4 +643,8 @@ rcl_ret_t rcl_node_type_description_service_init( 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..3641e618b 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; +#ifndef RCL_MICROROS rcutils_hash_map_t registered_types_by_type_hash; +#endif // RCL_MICROROS }; #endif // RCL__NODE_IMPL_H_ diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index 311824bbf..fca69b283 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" +#ifndef RCL_MICROROS #include "rcl/arguments.h" +#endif // RCL_MICROROS #include "rcl/domain_id.h" #include "rcl/error_handling.h" +#ifndef RCL_MICROROS #include "rcl/logging_rosout.h" +#endif // RCL_MICROROS #include "rmw/qos_profiles.h" @@ -35,9 +39,13 @@ rcl_node_get_default_options(void) rcl_node_options_t default_options = { .allocator = rcl_get_default_allocator(), .use_global_arguments = true, + #ifndef RCL_MICROROS .arguments = rcl_get_zero_initialized_arguments(), .enable_rosout = true, .rosout_qos = rmw_qos_profile_rosout_default, + #else + .enable_rosout = false, + #endif // RCL_MICROROS }; return default_options; } @@ -55,17 +63,21 @@ rcl_node_options_copy( RCL_SET_ERROR_MSG("Attempted to copy options into itself"); return RCL_RET_INVALID_ARGUMENT; } +#ifndef RCL_MICROROS if (NULL != options_out->arguments.impl) { RCL_SET_ERROR_MSG("Options out must be zero initialized"); return RCL_RET_INVALID_ARGUMENT; } +#endif // RCL_MICROROS 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; +#ifndef RCL_MICROROS if (NULL != options->arguments.impl) { return rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); } +#endif // RCL_MICROROS return RCL_RET_OK; } @@ -77,6 +89,7 @@ rcl_node_options_fini( rcl_allocator_t allocator = options->allocator; RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); +#ifndef RCL_MICROROS if (options->arguments.impl) { rcl_ret_t ret = rcl_arguments_fini(&options->arguments); if (RCL_RET_OK != ret) { @@ -84,6 +97,7 @@ rcl_node_options_fini( return ret; } } +#endif // RCL_MICROROS return RCL_RET_OK; } diff --git a/rcl/src/rcl/node_resolve_name.c b/rcl/src/rcl/node_resolve_name.c index c3b60317f..cd04c7e90 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 +#ifndef RCL_MICROROS 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 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; +#ifndef RCL_MICROROS if (node_options->use_global_arguments) { global_args = &(node->context->global_arguments); } +#endif // RCL_MICROROS return rcl_resolve_name( +#ifndef RCL_MICROROS &(node_options->arguments), +#else + NULL, +#endif // RCL_MICROROS 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 99067ccd3..98b2555f5 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -22,7 +22,9 @@ extern "C" #include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rcl/node.h" +#ifndef RCL_MICROROS #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rcl/time.h" @@ -127,6 +129,7 @@ rcl_publisher_init( // options publisher->impl->options = *options; +#ifndef RCL_MICROROS 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), @@ -137,6 +140,7 @@ rcl_publisher_init( goto fail; } publisher->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); // context @@ -199,6 +203,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; } +#ifndef RCL_MICROROS 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)) @@ -206,6 +211,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 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 f7e87c046..f44c31581 100644 --- a/rcl/src/rcl/publisher_impl.h +++ b/rcl/src/rcl/publisher_impl.h @@ -24,7 +24,9 @@ struct rcl_publisher_impl_s rmw_qos_profile_t actual_qos; rcl_context_t * context; rmw_publisher_t * rmw_handle; +#ifndef RCL_MICROROS rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS }; #endif // RCL__PUBLISHER_IMPL_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 97816a54d..b50b1663f 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" +#ifndef RCL_MICROROS #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS #include "rcl/publisher.h" #include "rcl/time.h" #include "rcl/types.h" @@ -181,6 +183,7 @@ rcl_service_init( service->impl->options = *options; service->impl->in_use_by_waitset = false; +#ifndef RCL_MICROROS 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), @@ -192,6 +195,7 @@ rcl_service_init( goto destroy_service; } service->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); TRACETOOLS_TRACEPOINT( @@ -255,6 +259,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) result = RCL_RET_ERROR; } +#ifndef RCL_MICROROS 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)) @@ -262,6 +267,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 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 2e36cbb6e..5da2933e4 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" +#ifndef RCL_MICROROS #include "rcl/node_type_cache.h" #include "rcutils/env.h" +#endif // RCL_MICROROS #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" #include "rcutils/types/string_array.h" @@ -128,6 +130,7 @@ rcl_subscription_init( subscription->impl->actual_qos.avoid_ros_namespace_conventions = options->qos.avoid_ros_namespace_conventions; +#ifndef RCL_MICROROS 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), @@ -138,6 +141,7 @@ rcl_subscription_init( goto fail; } subscription->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; @@ -210,6 +214,7 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) result = RCL_RET_ERROR; } +#ifndef RCL_MICROROS 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)) @@ -218,6 +223,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 allocator.deallocate(subscription->impl, allocator.state); subscription->impl = NULL; @@ -236,6 +242,7 @@ rcl_subscription_get_default_options(void) default_options.allocator = rcl_get_default_allocator(); default_options.rmw_subscription_options = rmw_get_default_subscription_options(); +#ifndef RCL_MICROROS // 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. @@ -251,6 +258,7 @@ rcl_subscription_get_default_options(void) } else { default_options.disable_loaned_message = !(strcmp(env_val, "0") == 0); } +#endif // RCL_MICROROS return default_options; } diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 7af04ed9b..ffe30e7eb 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; +#ifndef RCL_MICROROS rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS bool in_use_by_waitset; }; diff --git a/rcl/src/rcl/type_hash.c b/rcl/src/rcl/type_hash.c index 8c6b217d8..d52164a0e 100644 --- a/rcl/src/rcl/type_hash.c +++ b/rcl/src/rcl/type_hash.c @@ -14,7 +14,9 @@ #include +#ifndef RCL_MICROROS #include +#endif // RCL_MICROROS #include "rcl/allocator.h" #include "rcl/error_handling.h" @@ -25,6 +27,7 @@ #include "./common.h" +#ifndef RCL_MICROROS static int yaml_write_handler(void * ext, uint8_t * buffer, size_t size) { rcutils_char_array_t * repr = (rcutils_char_array_t *)ext; @@ -193,12 +196,14 @@ static int emit_type_description( } return end_sequence(emitter) && end_mapping(emitter); } +#endif // RCL_MICROROS rcl_ret_t rcl_type_description_to_hashable_json( const type_description_interfaces__msg__TypeDescription * type_description, rcutils_char_array_t * output_repr) { +#ifndef RCL_MICROROS RCL_CHECK_ARGUMENT_FOR_NULL(type_description, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(output_repr, RCL_RET_INVALID_ARGUMENT); @@ -240,6 +245,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 } rcl_ret_t @@ -247,6 +257,7 @@ rcl_calculate_type_hash( const type_description_interfaces__msg__TypeDescription * type_description, rosidl_type_hash_t * output_hash) { +#ifndef RCL_MICROROS RCL_CHECK_ARGUMENT_FOR_NULL(type_description, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(output_hash, RCL_RET_INVALID_ARGUMENT); @@ -270,4 +281,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 } diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index df4cbff30..f94978cef 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -28,7 +28,9 @@ extern "C" #include "rcl/client.h" #include "rcl/error_handling.h" #include "rcl/graph.h" +#ifndef RCL_MICROROS #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS #include "rcl/subscription.h" #include "rcl/types.h" #include "rcl/wait.h" @@ -66,7 +68,9 @@ _rcl_action_get_zero_initialized_client_impl(void) 0, 0, 0, +#ifndef RCL_MICROROS rosidl_get_zero_initialized_type_hash(), +#endif // RCL_MICROROS false }; return null_action_client; @@ -95,12 +99,14 @@ _rcl_action_client_fini_impl( if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->status_subscription, node)) { ret = RCL_RET_ERROR; } +#ifndef RCL_MICROROS 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 allocator.deallocate(action_client->impl->remapped_action_name, allocator.state); allocator.deallocate(action_client->impl, allocator.state); action_client->impl = NULL; @@ -234,16 +240,18 @@ rcl_action_client_init( SUBSCRIPTION_INIT(feedback); SUBSCRIPTION_INIT(status); +#ifndef RCL_MICROROS 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)); + 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; } action_client->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); return ret; @@ -341,41 +349,41 @@ rcl_action_server_is_available( // \internal Sends an action client specific service request. #define SEND_SERVICE_REQUEST(Type, request, sequence_number) \ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action " #Type " request"); \ - if (!rcl_action_client_is_valid(action_client)) { \ - return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ - } \ - RCL_CHECK_ARGUMENT_FOR_NULL(request, RCL_RET_INVALID_ARGUMENT); \ - RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT); \ - rcl_ret_t ret = rcl_send_request( \ - &action_client->impl->Type ## _client, request, sequence_number); \ - if (RCL_RET_OK != ret) { \ - return RCL_RET_ERROR; /* error already set */ \ - } \ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " request sent"); \ - return RCL_RET_OK; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action " #Type " request"); \ + if (!rcl_action_client_is_valid(action_client)) { \ + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ + } \ + RCL_CHECK_ARGUMENT_FOR_NULL(request, RCL_RET_INVALID_ARGUMENT); \ + RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT); \ + rcl_ret_t ret = rcl_send_request( \ + &action_client->impl->Type ## _client, request, sequence_number); \ + if (RCL_RET_OK != ret) { \ + return RCL_RET_ERROR; /* error already set */ \ + } \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " request sent"); \ + return RCL_RET_OK; // \internal Takes an action client specific service response. #define TAKE_SERVICE_RESPONSE(Type, response_header, response) \ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type " response"); \ - if (!rcl_action_client_is_valid(action_client)) { \ - return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ - } \ - RCL_CHECK_ARGUMENT_FOR_NULL(response_header, RCL_RET_INVALID_ARGUMENT); \ - RCL_CHECK_ARGUMENT_FOR_NULL(response, RCL_RET_INVALID_ARGUMENT); \ - rcl_ret_t ret = rcl_take_response( \ - &action_client->impl->Type ## _client, response_header, response); \ - if (RCL_RET_OK != ret) { \ - if (RCL_RET_BAD_ALLOC == ret) { \ - return RCL_RET_BAD_ALLOC; /* error already set */ \ - } \ - if (RCL_RET_CLIENT_TAKE_FAILED == ret) { \ - return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \ - } \ - return RCL_RET_ERROR; /* error already set */ \ - } \ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " response taken"); \ - return RCL_RET_OK; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type " response"); \ + if (!rcl_action_client_is_valid(action_client)) { \ + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ + } \ + RCL_CHECK_ARGUMENT_FOR_NULL(response_header, RCL_RET_INVALID_ARGUMENT); \ + RCL_CHECK_ARGUMENT_FOR_NULL(response, RCL_RET_INVALID_ARGUMENT); \ + rcl_ret_t ret = rcl_take_response( \ + &action_client->impl->Type ## _client, response_header, response); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_BAD_ALLOC == ret) { \ + return RCL_RET_BAD_ALLOC; /* error already set */ \ + } \ + if (RCL_RET_CLIENT_TAKE_FAILED == ret) { \ + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \ + } \ + return RCL_RET_ERROR; /* error already set */ \ + } \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " response taken"); \ + return RCL_RET_OK; rcl_ret_t @@ -434,25 +442,25 @@ rcl_action_take_cancel_response( // \internal Takes an action client specific topic message. #define TAKE_MESSAGE(Type) \ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type); \ - if (!rcl_action_client_is_valid(action_client)) { \ - return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ - } \ - RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type, RCL_RET_INVALID_ARGUMENT); \ - rmw_message_info_t message_info; /* ignored */ \ - rcl_ret_t ret = rcl_take( \ - &action_client->impl->Type ## _subscription, ros_ ## Type, &message_info, NULL); \ - if (RCL_RET_OK != ret) { \ - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { \ - return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \ - } \ - if (RCL_RET_BAD_ALLOC == ret) { \ - return RCL_RET_BAD_ALLOC; \ - } \ - return RCL_RET_ERROR; \ - } \ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " taken"); \ - return RCL_RET_OK; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type); \ + if (!rcl_action_client_is_valid(action_client)) { \ + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ + } \ + RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type, RCL_RET_INVALID_ARGUMENT); \ + rmw_message_info_t message_info; /* ignored */ \ + rcl_ret_t ret = rcl_take( \ + &action_client->impl->Type ## _subscription, ros_ ## Type, &message_info, NULL); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { \ + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \ + } \ + if (RCL_RET_BAD_ALLOC == ret) { \ + return RCL_RET_BAD_ALLOC; \ + } \ + return RCL_RET_ERROR; \ + } \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " taken"); \ + return RCL_RET_OK; rcl_ret_t rcl_action_take_feedback( @@ -754,16 +762,16 @@ rcl_action_client_set_status_subscription_callback( } #define CLIENT_CONFIGURE_SERVICE_INTROSPECTION(TYPE, STATE) \ - if (rcl_client_configure_service_introspection( \ - &action_client->impl->TYPE ## _client, \ - node, \ - clock, \ - type_support->TYPE ## _service_type_support, \ - publisher_options, \ - STATE) != RCL_RET_OK) \ - { \ - return RCL_RET_ERROR; \ - } + if (rcl_client_configure_service_introspection( \ + &action_client->impl->TYPE ## _client, \ + node, \ + clock, \ + type_support->TYPE ## _service_type_support, \ + publisher_options, \ + STATE) != RCL_RET_OK) \ + { \ + return RCL_RET_ERROR; \ + } rcl_ret_t @@ -862,16 +870,16 @@ _generate_goal_id_filter_expression( // Helper macro to append formatted string and check for errors #define APPEND_TO_EXPRESSION(...) \ - do { \ - int written = snprintf( \ - expression_goal_id_arrays + pos, expression_capacity - pos, __VA_ARGS__); \ - if (written < 0 || \ - written >= expression_capacity - pos || pos + written >= expression_capacity) { \ - allocator->deallocate(expression_goal_id_arrays, allocator->state); \ - return RCL_RET_ERROR; \ - } \ - pos += written; \ - } while (0) + do { \ + int written = snprintf( \ + expression_goal_id_arrays + pos, expression_capacity - pos, __VA_ARGS__); \ + if (written < 0 || \ + written >= expression_capacity - pos || pos + written >= expression_capacity) { \ + allocator->deallocate(expression_goal_id_arrays, allocator->state); \ + return RCL_RET_ERROR; \ + } \ + pos += written; \ + } while (0) // Traverse each goal ID array for (size_t i = 0; i < num_arrays; i++) { @@ -996,7 +1004,7 @@ rcl_action_client_configure_feedback_subscription_filter_add_goal_id( if (is_cft_enabled) { // Existing content filter and append new goal ID to it. ret = rcl_subscription_get_content_filter( - &action_client->impl->feedback_subscription, &content_filter_options); + &action_client->impl->feedback_subscription, &content_filter_options); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG("Failed to get cft expression parameters"); goto err; @@ -1151,7 +1159,7 @@ rcl_action_client_configure_feedback_subscription_filter_remove_goal_id( rcl_subscription_content_filter_options_t content_filter_options = rcl_get_zero_initialized_subscription_content_filter_options(); ret = rcl_subscription_get_content_filter( - &action_client->impl->feedback_subscription, &content_filter_options); + &action_client->impl->feedback_subscription, &content_filter_options); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG("Failed to get cft expression parameters"); return ret; @@ -1213,8 +1221,8 @@ rcl_action_client_configure_feedback_subscription_filter_remove_goal_id( // new_expression_params_size is guaranteed to be less than 100. new_expression_params = (char **)action_client->impl->options.allocator.allocate( - sizeof(char *) * new_expression_params_size, - action_client->impl->options.allocator.state); + sizeof(char *) * new_expression_params_size, + action_client->impl->options.allocator.state); if (new_expression_params == NULL) { RCL_SET_ERROR_MSG("Failed to allocate memory for expression parameters"); ret = RCL_RET_BAD_ALLOC; diff --git a/rcl_action/src/rcl_action/action_client_impl.h b/rcl_action/src/rcl_action/action_client_impl.h index 4eeb7e91b..e284fcaeb 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; +#ifndef RCL_MICROROS rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS /// Disable the content filter feature for feedback subscription. /** diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 9123e70c4..6ae4e3376 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -27,7 +27,9 @@ extern "C" #include "rcl_action/wait.h" #include "rcl/error_handling.h" +#ifndef RCL_MICROROS #include "rcl/node_type_cache.h" +#endif // RCL_MICROROS #include "rcl/rcl.h" #include "rcl/time.h" @@ -219,7 +221,9 @@ rcl_action_server_init2( action_server->impl->goal_handles = NULL; action_server->impl->num_goal_handles = 0u; action_server->impl->clock = NULL; +#ifndef RCL_MICROROS action_server->impl->type_hash = rosidl_get_zero_initialized_type_hash(); +#endif // RCL_MICROROS rcl_ret_t ret = RCL_RET_OK; @@ -279,6 +283,7 @@ rcl_action_server_init2( &_enqueue_check_expired_goals); (void) dont_care; +#ifndef RCL_MICROROS // Store type hash ret = rcl_node_type_cache_register_type( node, type_support->get_type_hash_func(type_support), @@ -291,6 +296,7 @@ rcl_action_server_init2( goto fail; } action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); +#endif // RCL_MICROROS return ret; fail: @@ -356,12 +362,14 @@ 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; +#ifndef RCL_MICROROS 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 // 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 8e6a1eb69..3be29971e 100644 --- a/rcl_action/src/rcl_action/action_server_impl.h +++ b/rcl_action/src/rcl_action/action_server_impl.h @@ -41,7 +41,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; +#ifndef RCL_MICROROS rosidl_type_hash_t type_hash; +#endif // RCL_MICROROS } rcl_action_server_impl_t;