From b3540685f61cb68da79ba7090b9232f72787de8d Mon Sep 17 00:00:00 2001 From: ylh <1076954547@qq.com> Date: Mon, 29 Jan 2024 11:41:17 +0800 Subject: [PATCH] origin rclcpp eloquent --- rclcpp-eloquent/.github/ISSUE_TEMPLATE.md | 45 + rclcpp-eloquent/.gitignore | 1 + rclcpp-eloquent/CONTRIBUTING.md | 18 + rclcpp-eloquent/LICENSE | 202 ++ rclcpp-eloquent/rclcpp/.gitignore | 1 + rclcpp-eloquent/rclcpp/CHANGELOG.rst | 276 ++ rclcpp-eloquent/rclcpp/CMakeLists.txt | 505 ++++ rclcpp-eloquent/rclcpp/Doxyfile | 33 + .../cmake/rclcpp_add_build_failure_test.cmake | 56 + .../rclcpp/allocator/allocator_common.hpp | 99 + .../rclcpp/allocator/allocator_deleter.hpp | 105 + .../rclcpp/include/rclcpp/any_executable.hpp | 57 + .../include/rclcpp/any_service_callback.hpp | 121 + .../rclcpp/any_subscription_callback.hpp | 267 ++ .../rclcpp/include/rclcpp/callback_group.hpp | 168 ++ .../rclcpp/include/rclcpp/client.hpp | 282 ++ .../rclcpp/include/rclcpp/clock.hpp | 143 + .../rclcpp/include/rclcpp/context.hpp | 370 +++ .../rclcpp/contexts/default_context.hpp | 45 + .../rclcpp/include/rclcpp/create_client.hpp | 56 + .../include/rclcpp/create_publisher.hpp | 71 + .../rclcpp/include/rclcpp/create_service.hpp | 58 + .../include/rclcpp/create_subscription.hpp | 81 + .../rclcpp/include/rclcpp/create_timer.hpp | 73 + .../rclcpp/include/rclcpp/detail/README.md | 3 + .../resolve_intra_process_buffer_type.hpp | 54 + .../detail/resolve_use_intra_process.hpp | 56 + .../rmw_implementation_specific_payload.hpp | 51 + ...lementation_specific_publisher_payload.hpp | 52 + ...entation_specific_subscription_payload.hpp | 53 + .../rclcpp/include/rclcpp/duration.hpp | 119 + .../rclcpp/include/rclcpp/event.hpp | 55 + .../rclcpp/include/rclcpp/exceptions.hpp | 260 ++ .../rclcpp/include/rclcpp/executor.hpp | 380 +++ .../rclcpp/include/rclcpp/executors.hpp | 126 + .../executors/multi_threaded_executor.hpp | 90 + .../executors/single_threaded_executor.hpp | 68 + .../rclcpp/expand_topic_or_service_name.hpp | 63 + .../include/rclcpp/experimental/README.md | 4 + .../buffers/buffer_implementation_base.hpp | 42 + .../buffers/intra_process_buffer.hpp | 241 ++ .../buffers/ring_buffer_implementation.hpp | 122 + .../create_intra_process_buffer.hpp | 99 + .../experimental/intra_process_manager.hpp | 426 +++ .../subscription_intra_process.hpp | 173 ++ .../subscription_intra_process_base.hpp | 88 + .../rclcpp/include/rclcpp/function_traits.hpp | 169 ++ .../rclcpp/include/rclcpp/graph_listener.hpp | 188 ++ .../rclcpp/include/rclcpp/init_options.hpp | 69 + .../rclcpp/intra_process_buffer_type.hpp | 35 + .../include/rclcpp/intra_process_setting.hpp | 34 + .../rclcpp/include/rclcpp/loaned_message.hpp | 207 ++ .../rclcpp/include/rclcpp/logger.hpp | 145 + .../rclcpp/include/rclcpp/macros.hpp | 114 + .../include/rclcpp/memory_strategies.hpp | 33 + .../rclcpp/include/rclcpp/memory_strategy.hpp | 149 + .../rclcpp/message_memory_strategy.hpp | 151 + .../rclcpp/include/rclcpp/node.hpp | 1117 ++++++++ .../rclcpp/include/rclcpp/node_impl.hpp | 272 ++ .../get_node_base_interface.hpp | 149 + .../get_node_timers_interface.hpp | 149 + .../get_node_topics_interface.hpp | 149 + .../rclcpp/node_interfaces/node_base.hpp | 159 ++ .../node_interfaces/node_base_interface.hpp | 169 ++ .../rclcpp/node_interfaces/node_clock.hpp | 72 + .../node_interfaces/node_clock_interface.hpp | 48 + .../rclcpp/node_interfaces/node_graph.hpp | 145 + .../node_interfaces/node_graph_interface.hpp | 158 ++ .../rclcpp/node_interfaces/node_logging.hpp | 66 + .../node_logging_interface.hpp | 58 + .../node_interfaces/node_parameters.hpp | 215 ++ .../node_parameters_interface.hpp | 213 ++ .../rclcpp/node_interfaces/node_services.hpp | 67 + .../node_services_interface.hpp | 57 + .../node_interfaces/node_time_source.hpp | 72 + .../node_time_source_interface.hpp | 42 + .../rclcpp/node_interfaces/node_timers.hpp | 60 + .../node_interfaces/node_timers_interface.hpp | 50 + .../rclcpp/node_interfaces/node_topics.hpp | 86 + .../node_interfaces/node_topics_interface.hpp | 86 + .../rclcpp/node_interfaces/node_waitables.hpp | 67 + .../node_waitables_interface.hpp | 57 + .../rclcpp/include/rclcpp/node_options.hpp | 336 +++ .../rclcpp/include/rclcpp/parameter.hpp | 245 ++ .../include/rclcpp/parameter_client.hpp | 348 +++ .../rclcpp/parameter_events_filter.hpp | 82 + .../rclcpp/include/rclcpp/parameter_map.hpp | 53 + .../include/rclcpp/parameter_service.hpp | 63 + .../rclcpp/include/rclcpp/parameter_value.hpp | 345 +++ .../rclcpp/include/rclcpp/publisher.hpp | 350 +++ .../rclcpp/include/rclcpp/publisher_base.hpp | 228 ++ .../include/rclcpp/publisher_factory.hpp | 91 + .../include/rclcpp/publisher_options.hpp | 110 + rclcpp-eloquent/rclcpp/include/rclcpp/qos.hpp | 206 ++ .../rclcpp/include/rclcpp/qos_event.hpp | 126 + .../rclcpp/include/rclcpp/rate.hpp | 119 + .../rclcpp/include/rclcpp/rclcpp.hpp | 157 ++ .../rclcpp/include/rclcpp/scope_exit.hpp | 52 + .../rclcpp/include/rclcpp/service.hpp | 260 ++ .../strategies/allocator_memory_strategy.hpp | 508 ++++ .../message_pool_memory_strategy.hpp | 112 + .../rclcpp/include/rclcpp/subscription.hpp | 276 ++ .../include/rclcpp/subscription_base.hpp | 236 ++ .../include/rclcpp/subscription_factory.hpp | 123 + .../include/rclcpp/subscription_options.hpp | 107 + .../include/rclcpp/subscription_traits.hpp | 97 + .../rclcpp/include/rclcpp/time.hpp | 130 + .../rclcpp/include/rclcpp/time_source.hpp | 143 + .../rclcpp/include/rclcpp/timer.hpp | 233 ++ .../include/rclcpp/type_support_decl.hpp | 78 + .../rclcpp/include/rclcpp/utilities.hpp | 307 ++ .../include/rclcpp/visibility_control.hpp | 56 + .../rclcpp/include/rclcpp/waitable.hpp | 153 + rclcpp-eloquent/rclcpp/package.xml | 46 + .../rclcpp/resource/logging.hpp.em | 166 ++ .../rclcpp/src/rclcpp/any_executable.cpp | 36 + .../rclcpp/src/rclcpp/callback_group.cpp | 116 + rclcpp-eloquent/rclcpp/src/rclcpp/client.cpp | 179 ++ rclcpp-eloquent/rclcpp/src/rclcpp/clock.cpp | 144 + rclcpp-eloquent/rclcpp/src/rclcpp/context.cpp | 309 ++ .../src/rclcpp/contexts/default_context.cpp | 27 + .../rmw_implementation_specific_payload.cpp | 35 + ...lementation_specific_publisher_payload.cpp | 33 + ...entation_specific_subscription_payload.cpp | 33 + .../rclcpp/src/rclcpp/duration.cpp | 242 ++ rclcpp-eloquent/rclcpp/src/rclcpp/event.cpp | 41 + .../rclcpp/src/rclcpp/exceptions.cpp | 146 + .../rclcpp/src/rclcpp/executor.cpp | 631 +++++ .../rclcpp/src/rclcpp/executors.cpp | 43 + .../executors/multi_threaded_executor.cpp | 116 + .../executors/single_threaded_executor.cpp | 39 + .../rclcpp/expand_topic_or_service_name.cpp | 203 ++ .../rclcpp/src/rclcpp/graph_listener.cpp | 404 +++ .../rclcpp/src/rclcpp/init_options.cpp | 89 + .../src/rclcpp/intra_process_manager.cpp | 227 ++ rclcpp-eloquent/rclcpp/src/rclcpp/logger.cpp | 47 + .../rclcpp/src/rclcpp/memory_strategies.cpp | 27 + .../rclcpp/src/rclcpp/memory_strategy.cpp | 284 ++ rclcpp-eloquent/rclcpp/src/rclcpp/node.cpp | 489 ++++ .../src/rclcpp/node_interfaces/node_base.cpp | 270 ++ .../src/rclcpp/node_interfaces/node_clock.cpp | 43 + .../src/rclcpp/node_interfaces/node_graph.cpp | 373 +++ .../rclcpp/node_interfaces/node_logging.cpp | 39 + .../node_interfaces/node_parameters.cpp | 890 ++++++ .../rclcpp/node_interfaces/node_services.cpp | 80 + .../node_interfaces/node_time_source.cpp | 50 + .../rclcpp/node_interfaces/node_timers.cpp | 47 + .../rclcpp/node_interfaces/node_topics.cpp | 123 + .../rclcpp/node_interfaces/node_waitables.cpp | 68 + .../rclcpp/src/rclcpp/node_options.cpp | 341 +++ .../rclcpp/src/rclcpp/parameter.cpp | 215 ++ .../rclcpp/src/rclcpp/parameter_client.cpp | 505 ++++ .../src/rclcpp/parameter_events_filter.cpp | 60 + .../rclcpp/src/rclcpp/parameter_map.cpp | 128 + .../rclcpp/src/rclcpp/parameter_service.cpp | 154 + .../src/rclcpp/parameter_service_names.hpp | 33 + .../rclcpp/src/rclcpp/parameter_value.cpp | 241 ++ .../rclcpp/src/rclcpp/publisher_base.cpp | 246 ++ rclcpp-eloquent/rclcpp/src/rclcpp/qos.cpp | 207 ++ .../rclcpp/src/rclcpp/qos_event.cpp | 55 + rclcpp-eloquent/rclcpp/src/rclcpp/service.cpp | 65 + .../rclcpp/src/rclcpp/signal_handler.cpp | 379 +++ .../rclcpp/src/rclcpp/signal_handler.hpp | 189 ++ .../rclcpp/src/rclcpp/subscription_base.cpp | 211 ++ .../subscription_intra_process_base.cpp | 38 + rclcpp-eloquent/rclcpp/src/rclcpp/time.cpp | 265 ++ .../rclcpp/src/rclcpp/time_source.cpp | 333 +++ rclcpp-eloquent/rclcpp/src/rclcpp/timer.cpp | 130 + .../rclcpp/src/rclcpp/type_support.cpp | 116 + .../rclcpp/src/rclcpp/utilities.cpp | 188 ++ .../rclcpp/src/rclcpp/waitable.cpp | 53 + .../test_multi_threaded_executor.cpp | 102 + .../test/node_interfaces/node_wrapper.hpp | 64 + ...topics_interface_const_ptr_rclcpp_node.cpp | 28 + ...opics_interface_const_ptr_wrapped_node.cpp | 30 + ...topics_interface_const_ref_rclcpp_node.cpp | 28 + ...opics_interface_const_ref_wrapped_node.cpp | 30 + .../test_get_node_interfaces.cpp | 117 + .../resources/test_node/test_parameters.yaml | 4 + rclcpp-eloquent/rclcpp/test/test_client.cpp | 125 + .../rclcpp/test/test_create_timer.cpp | 63 + rclcpp-eloquent/rclcpp/test/test_duration.cpp | 155 + rclcpp-eloquent/rclcpp/test/test_executor.cpp | 69 + .../test_expand_topic_or_service_name.cpp | 80 + .../test/test_externally_defined_services.cpp | 160 ++ .../rclcpp/test/test_find_weak_nodes.cpp | 82 + .../rclcpp/test/test_function_traits.cpp | 753 +++++ rclcpp-eloquent/rclcpp/test/test_init.cpp | 29 + .../rclcpp/test/test_intra_process_buffer.cpp | 240 ++ .../test/test_intra_process_manager.cpp | 672 +++++ .../rclcpp/test/test_loaned_message.cpp | 83 + rclcpp-eloquent/rclcpp/test/test_logger.cpp | 35 + rclcpp-eloquent/rclcpp/test/test_logging.cpp | 275 ++ rclcpp-eloquent/rclcpp/test/test_node.cpp | 2508 +++++++++++++++++ .../rclcpp/test/test_node_global_args.cpp | 60 + .../rclcpp/test/test_node_options.cpp | 100 + .../rclcpp/test/test_parameter.cpp | 769 +++++ .../rclcpp/test/test_parameter_client.cpp | 157 ++ .../test/test_parameter_events_filter.cpp | 205 ++ .../rclcpp/test/test_parameter_map.cpp | 355 +++ .../rclcpp/test/test_publisher.cpp | 205 ++ .../test_publisher_subscription_count_api.cpp | 178 ++ rclcpp-eloquent/rclcpp/test/test_rate.cpp | 100 + .../test/test_ring_buffer_implementation.cpp | 81 + .../test_serialized_message_allocator.cpp | 68 + rclcpp-eloquent/rclcpp/test/test_service.cpp | 107 + .../rclcpp/test/test_subscription.cpp | 297 ++ .../test_subscription_publisher_count_api.cpp | 123 + .../rclcpp/test/test_subscription_traits.cpp | 178 ++ rclcpp-eloquent/rclcpp/test/test_time.cpp | 254 ++ .../rclcpp/test/test_time_source.cpp | 515 ++++ rclcpp-eloquent/rclcpp/test/test_timer.cpp | 153 + .../rclcpp/test/test_utilities.cpp | 80 + rclcpp-eloquent/rclcpp_action/CHANGELOG.rst | 79 + rclcpp-eloquent/rclcpp_action/CMakeLists.txt | 102 + rclcpp-eloquent/rclcpp_action/Doxyfile | 35 + .../include/rclcpp_action/client.hpp | 647 +++++ .../rclcpp_action/client_goal_handle.hpp | 165 ++ .../rclcpp_action/client_goal_handle_impl.hpp | 169 ++ .../include/rclcpp_action/create_client.hpp | 117 + .../include/rclcpp_action/create_server.hpp | 155 + .../include/rclcpp_action/exceptions.hpp | 46 + .../include/rclcpp_action/qos.hpp | 34 + .../include/rclcpp_action/rclcpp_action.hpp | 44 + .../include/rclcpp_action/server.hpp | 486 ++++ .../rclcpp_action/server_goal_handle.hpp | 284 ++ .../include/rclcpp_action/types.hpp | 84 + .../rclcpp_action/visibility_control.hpp | 56 + rclcpp-eloquent/rclcpp_action/package.xml | 32 + rclcpp-eloquent/rclcpp_action/src/client.cpp | 444 +++ rclcpp-eloquent/rclcpp_action/src/qos.cpp | 28 + rclcpp-eloquent/rclcpp_action/src/server.cpp | 560 ++++ .../rclcpp_action/src/server_goal_handle.cpp | 140 + rclcpp-eloquent/rclcpp_action/src/types.cpp | 48 + .../rclcpp_action/test/test_client.cpp | 666 +++++ .../rclcpp_action/test/test_server.cpp | 792 ++++++ .../rclcpp_action/test/test_traits.cpp | 99 + .../rclcpp_components/CHANGELOG.rst | 79 + .../rclcpp_components/CMakeLists.txt | 126 + rclcpp-eloquent/rclcpp_components/Doxyfile | 33 + .../rclcpp_components_package_hook.cmake | 18 + .../rclcpp_components_register_node.cmake | 52 + .../rclcpp_components_register_nodes.cmake | 62 + .../rclcpp_components/node_factory.hpp | 46 + .../node_factory_template.hpp | 53 + .../node_instance_wrapper.hpp | 71 + .../rclcpp_components/register_node_macro.hpp | 38 + rclcpp-eloquent/rclcpp_components/package.xml | 33 + .../rclcpp_components-extras.cmake.in | 32 + .../src/component_container.cpp | 29 + .../src/component_container_mt.cpp | 29 + .../src/component_manager.cpp | 263 ++ .../src/component_manager.hpp | 104 + .../rclcpp_components/src/node_main.cpp.in | 66 + .../test/components/test_component.cpp | 68 + .../test/test_component_manager.cpp | 91 + .../test/test_component_manager_api.cpp | 243 ++ .../rclcpp_lifecycle/CHANGELOG.rst | 108 + .../rclcpp_lifecycle/CMakeLists.txt | 109 + .../rclcpp_lifecycle/lifecycle_node.hpp | 668 +++++ .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 235 ++ .../rclcpp_lifecycle/lifecycle_publisher.hpp | 138 + .../lifecycle_node_interface.hpp | 107 + .../include/rclcpp_lifecycle/state.hpp | 77 + .../include/rclcpp_lifecycle/transition.hpp | 92 + .../type_traits/is_manageable_node.hpp | 62 + .../rclcpp_lifecycle/visibility_control.h | 56 + rclcpp-eloquent/rclcpp_lifecycle/package.xml | 31 + .../rclcpp_lifecycle/src/lifecycle_node.cpp | 567 ++++ .../src/lifecycle_node_interface_impl.hpp | 527 ++++ .../lifecycle_node_interface.cpp | 61 + .../rclcpp_lifecycle/src/state.cpp | 164 ++ .../rclcpp_lifecycle/src/transition.cpp | 267 ++ .../test/test_callback_exceptions.cpp | 128 + .../test/test_lifecycle_node.cpp | 235 ++ .../test/test_register_custom_callbacks.cpp | 180 ++ .../test/test_state_machine_info.cpp | 75 + .../test/test_state_wrapper.cpp | 168 ++ .../test/test_transition_wrapper.cpp | 86 + 279 files changed, 47558 insertions(+) create mode 100644 rclcpp-eloquent/.github/ISSUE_TEMPLATE.md create mode 100644 rclcpp-eloquent/.gitignore create mode 100644 rclcpp-eloquent/CONTRIBUTING.md create mode 100644 rclcpp-eloquent/LICENSE create mode 100644 rclcpp-eloquent/rclcpp/.gitignore create mode 100644 rclcpp-eloquent/rclcpp/CHANGELOG.rst create mode 100644 rclcpp-eloquent/rclcpp/CMakeLists.txt create mode 100644 rclcpp-eloquent/rclcpp/Doxyfile create mode 100644 rclcpp-eloquent/rclcpp/cmake/rclcpp_add_build_failure_test.cmake create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_common.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/any_executable.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/any_service_callback.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/any_subscription_callback.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/callback_group.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/client.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/clock.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/context.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/contexts/default_context.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/create_client.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/create_publisher.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/create_service.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/create_subscription.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/create_timer.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/detail/README.md create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/duration.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/event.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/exceptions.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/executor.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/executors.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/README.md create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/function_traits.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/graph_listener.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/init_options.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_buffer_type.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_setting.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/loaned_message.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/logger.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/macros.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategies.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategy.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/message_memory_strategy.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_impl.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/node_options.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/parameter.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/parameter_client.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/parameter_events_filter.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/parameter_map.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/parameter_service.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/parameter_value.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/publisher.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/publisher_base.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/publisher_factory.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/publisher_options.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/qos.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/qos_event.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/rate.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/rclcpp.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/scope_exit.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/service.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/subscription.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/subscription_base.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/subscription_factory.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/subscription_options.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/subscription_traits.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/time.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/time_source.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/timer.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/type_support_decl.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/utilities.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/visibility_control.hpp create mode 100644 rclcpp-eloquent/rclcpp/include/rclcpp/waitable.hpp create mode 100644 rclcpp-eloquent/rclcpp/package.xml create mode 100644 rclcpp-eloquent/rclcpp/resource/logging.hpp.em create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/any_executable.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/callback_group.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/client.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/clock.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/context.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/contexts/default_context.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/duration.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/event.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/exceptions.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/executor.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/executors.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/graph_listener.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/init_options.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/intra_process_manager.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/logger.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategies.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategy.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_base.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_services.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/node_options.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/parameter.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/parameter_client.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/parameter_events_filter.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/parameter_map.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service_names.hpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/parameter_value.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/publisher_base.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/qos.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/qos_event.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/service.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.hpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/subscription_base.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/subscription_intra_process_base.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/time.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/time_source.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/timer.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/type_support.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/utilities.cpp create mode 100644 rclcpp-eloquent/rclcpp/src/rclcpp/waitable.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/executors/test_multi_threaded_executor.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/node_interfaces/node_wrapper.hpp create mode 100644 rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/resources/test_node/test_parameters.yaml create mode 100644 rclcpp-eloquent/rclcpp/test/test_client.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_create_timer.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_duration.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_executor.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_expand_topic_or_service_name.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_externally_defined_services.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_find_weak_nodes.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_function_traits.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_init.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_intra_process_buffer.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_intra_process_manager.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_loaned_message.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_logger.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_logging.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_node.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_node_global_args.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_node_options.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_parameter.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_parameter_client.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_parameter_events_filter.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_parameter_map.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_publisher.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_publisher_subscription_count_api.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_rate.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_ring_buffer_implementation.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_serialized_message_allocator.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_service.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_subscription.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_subscription_publisher_count_api.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_subscription_traits.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_time.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_time_source.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_timer.cpp create mode 100644 rclcpp-eloquent/rclcpp/test/test_utilities.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/CHANGELOG.rst create mode 100644 rclcpp-eloquent/rclcpp_action/CMakeLists.txt create mode 100644 rclcpp-eloquent/rclcpp_action/Doxyfile create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_client.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_server.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/exceptions.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/qos.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/types.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/include/rclcpp_action/visibility_control.hpp create mode 100644 rclcpp-eloquent/rclcpp_action/package.xml create mode 100644 rclcpp-eloquent/rclcpp_action/src/client.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/src/qos.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/src/server.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/src/server_goal_handle.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/src/types.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/test/test_client.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/test/test_server.cpp create mode 100644 rclcpp-eloquent/rclcpp_action/test/test_traits.cpp create mode 100644 rclcpp-eloquent/rclcpp_components/CHANGELOG.rst create mode 100644 rclcpp-eloquent/rclcpp_components/CMakeLists.txt create mode 100644 rclcpp-eloquent/rclcpp_components/Doxyfile create mode 100644 rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_package_hook.cmake create mode 100644 rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_node.cmake create mode 100644 rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake create mode 100644 rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory.hpp create mode 100644 rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory_template.hpp create mode 100644 rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp create mode 100644 rclcpp-eloquent/rclcpp_components/include/rclcpp_components/register_node_macro.hpp create mode 100644 rclcpp-eloquent/rclcpp_components/package.xml create mode 100644 rclcpp-eloquent/rclcpp_components/rclcpp_components-extras.cmake.in create mode 100644 rclcpp-eloquent/rclcpp_components/src/component_container.cpp create mode 100644 rclcpp-eloquent/rclcpp_components/src/component_container_mt.cpp create mode 100644 rclcpp-eloquent/rclcpp_components/src/component_manager.cpp create mode 100644 rclcpp-eloquent/rclcpp_components/src/component_manager.hpp create mode 100644 rclcpp-eloquent/rclcpp_components/src/node_main.cpp.in create mode 100644 rclcpp-eloquent/rclcpp_components/test/components/test_component.cpp create mode 100644 rclcpp-eloquent/rclcpp_components/test/test_component_manager.cpp create mode 100644 rclcpp-eloquent/rclcpp_components/test/test_component_manager_api.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/CHANGELOG.rst create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/CMakeLists.txt create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/package.xml create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/src/state.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/src/transition.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/test/test_callback_exceptions.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/test/test_lifecycle_node.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/test/test_state_machine_info.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/test/test_state_wrapper.cpp create mode 100644 rclcpp-eloquent/rclcpp_lifecycle/test/test_transition_wrapper.cpp diff --git a/rclcpp-eloquent/.github/ISSUE_TEMPLATE.md b/rclcpp-eloquent/.github/ISSUE_TEMPLATE.md new file mode 100644 index 0000000..f705392 --- /dev/null +++ b/rclcpp-eloquent/.github/ISSUE_TEMPLATE.md @@ -0,0 +1,45 @@ + + +## Bug report + +**Required Info:** + +- Operating System: + - +- Installation type: + - +- Version or commit hash: + - +- DDS implementation: + - +- Client library (if applicable): + - + +#### Steps to reproduce issue + +``` + +``` + +#### Expected behavior + +#### Actual behavior + +#### Additional information + + +---- +## Feature request + +#### Feature description + + +#### Implementation considerations + diff --git a/rclcpp-eloquent/.gitignore b/rclcpp-eloquent/.gitignore new file mode 100644 index 0000000..e43b0f9 --- /dev/null +++ b/rclcpp-eloquent/.gitignore @@ -0,0 +1 @@ +.DS_Store diff --git a/rclcpp-eloquent/CONTRIBUTING.md b/rclcpp-eloquent/CONTRIBUTING.md new file mode 100644 index 0000000..cfba094 --- /dev/null +++ b/rclcpp-eloquent/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/rclcpp-eloquent/LICENSE b/rclcpp-eloquent/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/rclcpp-eloquent/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/rclcpp-eloquent/rclcpp/.gitignore b/rclcpp-eloquent/rclcpp/.gitignore new file mode 100644 index 0000000..94897f6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/.gitignore @@ -0,0 +1 @@ +doc_output diff --git a/rclcpp-eloquent/rclcpp/CHANGELOG.rst b/rclcpp-eloquent/rclcpp/CHANGELOG.rst new file mode 100644 index 0000000..60073f2 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/CHANGELOG.rst @@ -0,0 +1,276 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-12-04) +------------------ +* Warn about unused result of add_on_set_parameters_callback (`#1238 `_) (`#1243 `_) +* fix exception message on rcl_clock_init. (`#1194 `_) +* Check if context is valid when looping in spin_some (`#1167 `_) +* Fix spin_until_future_complete: check spinning value (`#1023 `_) +* Fix lock-order-inversion (potential deadlock) (`#1135 `_) (`#1137 `_) +* Don't specify calling convention in std::_Binder template (`#952 `_) (`#1006 `_) +* Add missing service callback registration tracepoint (`#986 `_) (`#1004 `_) +* Allow node clock use in logging macros (`#969 `_) (`#970 `_) (`#981 `_) +* Complete published event message when declaring a parameter (`#928 `_) (`#966 `_) +* Contributors: Christophe Bedard, DongheeYe, Ivan Santiago Paunovic, Jacob Perron, Sean Kelly, Shane Loretz, tomoya + +0.8.4 (2020-01-17) +------------------ +* Intra-process subscriber should use RMW actual qos (ros2`#913 `_) (`#914 `_) (`#965 `_) +* Contributors: Todd Malsbary + +0.8.3 (2019-11-19) +------------------ + +0.8.2 (2019-11-18) +------------------ +* Updated tracing logic to match changes in rclcpp's intra-process system (`#918 `_) +* Fixed a bug that prevented the ``shutdown_on_sigint`` option to not work correctly (`#850 `_) +* Added support for STREAM logging macros (`#926 `_) +* Relaxed multithreaded test constraint (`#907 `_) +* Contributors: Anas Abou Allaban, Christophe Bedard, Dirk Thomas, alexfneves + +0.8.1 (2019-10-23) +------------------ +* De-flake tests for rmw_connext (`#899 `_) +* rename return functions for loaned messages (`#896 `_) +* Enable throttling logs (`#879 `_) +* New Intra-Process Communication (`#778 `_) +* Instrumentation update (`#789 `_) +* Zero copy api (`#864 `_) +* Drop rclcpp remove_ros_arguments_null test case. (`#894 `_) +* add mechanism to pass rmw impl specific payloads during pub/sub creation (`#882 `_) +* make get_actual_qos return a rclcpp::QoS (`#883 `_) +* Fix Compiler Warning (`#881 `_) +* Add callback handler for use_sim_time parameter `#802 `_ (`#875 `_) +* Contributors: Alberto Soragna, Brian Marchi, Hunter L. Allen, Ingo Lütkebohle, Karsten Knese, Michael Carroll, Michel Hidalgo, William Woodall + +0.8.0 (2019-09-26) +------------------ +* clean up publisher and subscription creation logic (`#867 `_) +* Take parameter overrides provided through the CLI. (`#865 `_) +* add more context to exception message (`#858 `_) +* remove features and related code which were deprecated in dashing (`#852 `_) +* check valid timer handler 1st to reduce the time window for scan. (`#841 `_) +* Add throwing parameter name if parameter is not set (`#833 `_) +* Fix typo in deprecated warning. (`#848 `_) +* Fail on invalid and unknown ROS specific arguments (`#842 `_) +* Force explicit --ros-args in NodeOptions::arguments(). (`#845 `_) +* Use of -r/--remap flags where appropriate. (`#834 `_) +* Fix hang with timers in MultiThreadedExecutor (`#835 `_) (`#836 `_) +* add mutex in add/remove_node and wait_for_work to protect concurrent use/change of memory_strategy\_ (`#837 `_) +* Crash in callback group pointer vector iterator (`#814 `_) +* Wrap documentation examples in code blocks (`#830 `_) +* add callback group as member variable and constructor arg (`#811 `_) +* Fix get_node_interfaces functions taking a pointer (`#821 `_) +* Delete unnecessary call for get_node_by_group (`#823 `_) +* Allow passing logger by const ref (`#820 `_) +* Explain return value of spin_until_future_complete (`#792 `_) +* Adapt to '--ros-args ... [--]'-based ROS args extraction (`#816 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* remove mock msgs from rclcpp (`#800 `_) +* Make TimeSource ignore use_sim_time events coming from other nodes. (`#799 `_) +* Allow registering multiple on_parameters_set_callback (`#772 `_) +* Add free function for creating service clients (`#788 `_) +* Include missing rcl headers in use. (`#782 `_) +* Switch the NodeParameters lock to recursive. (`#781 `_) +* changed on_parameter_event qos profile to rmw_qos_profile_parameter_events (`#774 `_) +* Adding a factory method to create a Duration from seconds (`#567 `_) +* Fix a comparison with a sign mismatch (`#771 `_) +* delete superfluous spaces (`#770 `_) +* Use params from node '/\*\*' from parameter YAML file (`#762 `_) +* Add ignore override argument to declare parameter (`#767 `_) +* use default parameter descriptor in parameters interface (`#765 `_) +* Added support for const member functions (`#763 `_) +* add get_actual_qos() feature to subscriptions (`#754 `_) +* Ignore parameters overrides in set parameter methods when allowing undeclared parameters (`#756 `_) +* Add rclcpp::create_timer() (`#757 `_) +* checking origin of intra-process msg before taking them (`#753 `_) +* Contributors: Alberto Soragna, Carl Delsey, Chris Lalancette, Dan Rose, Dirk Thomas, Esteve Fernandez, Guillaume Autran, Jacob Perron, Karsten Knese, Luca Della Vedova, M. M, Michel Hidalgo, Scott K Logan, Shane Loretz, Todd Malsbary, William Woodall, bpwilcox, fujitatomoya, ivanpauno + +0.7.5 (2019-05-30) +------------------ +* Avoid 'Intra process message no longer being stored when trying to handle it' warning (`#749 `_) +* Contributors: ivanpauno + +0.7.4 (2019-05-29) +------------------ +* Rename parameter options (`#745 `_) +* Bionic use of strerror_r (`#742 `_) +* Enforce parameter ranges (`#735 `_) +* removed not used parameter client (`#740 `_) +* ensure removal of guard conditions of expired nodes from memory strategy (`#741 `_) +* Fix typo in log warning message (`#737 `_) +* Throw nice errors when creating a publisher with intraprocess communication and incompatible qos policy (`#729 `_) +* Contributors: Alberto Soragna, Dirk Thomas, Jacob Perron, William Woodall, ivanpauno, roderick-koehle + +0.7.3 (2019-05-20) +------------------ +* Fixed misspelling, volitile -> volatile (`#724 `_), and then fixed that since it is a C++ keyword to be ``durability_volatile`` (`#725 `_) +* Fixed a clang warning (`#723 `_) +* Added ``on_parameter_event`` static method to the ``AsyncParametersClient`` (`#688 `_) +* Added a guard against ``ParameterNotDeclaredException`` throwing from within the parameter service callbacks. (`#718 `_) +* Added missing template functionality to lifecycle_node. (`#707 `_) +* Fixed heap-use-after-free and memory leaks reported from ``test_node.cpp`` (`#719 `_) +* Contributors: Alberto Soragna, Dirk Thomas, Emerson Knapp, Jacob Perron, Michael Jeronimo, Prajakta Gokhale + +0.7.2 (2019-05-08) +------------------ +* Added new way to specify QoS settings for publishers and subscriptions. (`#713 `_) + * The new way requires that you specify a history depth when creating a publisher or subscription. + * In the past it was possible to create one without specifying any history depth, but these signatures have been deprecated. +* Deprecated ``shared_ptr`` and raw pointer versions of ``Publisher::publish()``. (`#709 `_) +* Implemented API to set callbacks for liveliness and deadline QoS events for publishers and subscriptions. (`#695 `_) +* Fixed a segmentation fault when publishing a parameter event when they ought to be disabled. (`#714 `_) +* Changes required for upcoming pre-allocation API. (`#711 `_) +* Changed ``Node::get_node_names()`` to return the full node names rather than just the base name. (`#698 `_) +* Remove logic made redundant by the `ros2/rcl#255 `_ pull request. (`#712 `_) +* Various improvements for ``rclcpp::Clock``. (`#696 `_) + * Fixed uninitialized bool in ``clock.cpp``. + * Fixed up includes of ``clock.hpp/cpp``. + * Added documentation for exceptions to ``clock.hpp``. + * Adjusted function signature of getters of ``clock.hpp/cpp``. + * Removed raw pointers to ``Clock::create_jump_callback``. + * Removed unnecessary ``rclcpp`` namespace reference from ``clock.cpp``. + * Changed exception to ``bad_alloc`` on ``JumpHandler`` allocation failure. + * Fixed missing ``nullptr`` check in ``Clock::on_time_jump``. + * Added ``JumpHandler::callback`` types. + * Added warning for lifetime of Clock and JumpHandler +* Fixed bug left over from the `pull request #495 `_. (`#708 `_) +* Changed the ``IntraProcessManager`` to be capable of storing ``shared_ptr`` in addition to ``unique_ptr``. (`#690 `_) +* Contributors: Alberto Soragna, Dima Dorezyuk, M. M, Michael Carroll, Michael Jeronimo, Tully Foote, William Woodall, ivanpauno, jhdcs + +0.7.1 (2019-04-26) +------------------ +* Added read only parameters. (`#495 `_) +* Fixed a concurrency problem in the multithreaded executor. (`#703 `_) +* Fixup utilities. (`#692 `_) +* Added method to read timer cancellation. (`#697 `_) +* Added Exception Generator function for implementing "from_rcl_error". (`#678 `_) +* Updated initialization of rmw_qos_profile_t struct instances. (`#691 `_) +* Removed the const value from the logger before comparison. (`#680 `_) +* Contributors: Devin Bonnie, Dima Dorezyuk, Guillaume Autran, M. M, Shane Loretz, Víctor Mayoral Vilches, William Woodall, jhdcs + +0.7.0 (2019-04-14) +------------------ +* Added Options-struct interfaces for creating publishers/subscribers (pre-QoS, standalone). (`#673 `_) +* Replaced strncpy with memcpy. (`#684 `_) +* Replaced const char * with a std::array as the key of IPM IDTopicMap. (`#671 `_) +* Refactored SignalHandler logger to avoid race during destruction. (`#682 `_) +* Introduce rclcpp_components to implement composition. (`#665 `_) +* Added QoS policy check when configuring intraprocess, skip interprocess publish when possible. (`#674 `_) +* Updated to use do { .. } while(0) around content of logging macros. (`#681 `_) +* Added function to get publisher's actual QoS settings. (`#667 `_) +* Updated to avoid race that triggers timer too often. (`#621 `_) +* Exposed get_fully_qualified_name in NodeBase API. (`#662 `_) +* Updated to use ament_target_dependencies where possible. (`#659 `_) +* Fixed wait for service memory leak bug. (`#656 `_) +* Fixed test_time_source test. (`#639 `_) +* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 `_) +* Fixed cppcheck warning. (`#646 `_) +* Added count matching api and intra-process subscriber count. (`#628 `_) +* Added Sub Node alternative. (`#581 `_) +* Replaced 'auto' with 'const auto &'. (`#630 `_) +* Set Parameter Event Publisher settings. `#591 `_ (`#614 `_) +* Replaced node constructor arguments with NodeOptions. (`#622 `_) +* Updated to pass context to wait set (`#617 `_) +* Added API to get parameters in a map. (`#575 `_) +* Updated Bind usage since it is is no longer in std::__1. (`#618 `_) +* Fixed errors from uncrustify v0.68. (`#613 `_) +* Added new constructors for SyncParameterClient. (`#612 `_) +* Contributors: Alberto Soragna, Chris Lalancette, Dirk Thomas, Emerson Knapp, Francisco Martín Rico, Jacob Perron, Marko Durkovic, Michael Carroll, Peter Baughman, Shane Loretz, Wei Liu, William Woodall, Yutaka Kondo, ivanpauno, kuzai, rarvolt + +0.6.2 (2018-12-13) +------------------ +* Updated to use signal safe synchronization with platform specific semaphores (`#607 `_) +* Resolved startup race condition for sim time (`#608 `_) + Resolves `#595 `_ +* Contributors: Tully Foote, William Woodall + +0.6.1 (2018-12-07) +------------------ +* Added wait_for_action_server() for action clients (`#598 `_) +* Added node path and time stamp to parameter event message (`#584 `_) +* Updated to allow removing a waitable (`#597 `_) +* Refactored init to allow for non-global init (`#587 `_) +* Fixed wrong use of constructor and hanging test (`#596 `_) +* Added class Waitable (`#589 `_) +* Updated rcl_wait_set_add\_* calls (`#586 `_) +* Contributors: Dirk Thomas, Jacob Perron, Shane Loretz, William Woodall, bpwilcox + +0.6.0 (2018-11-19) +------------------ +* Updated to use new error handling API from rcutils (`#577 `_) +* Added a warning when publishing if publisher is not active (`#574 `_) +* Added logging macro signature that accepts std::string (`#573 `_) +* Added virtual destructors to classes with virtual functions. (`#566 `_) +* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 `_) +* Removed std::binary_function usage (`#561 `_) +* Updated to avoid auto-activating ROS time if clock topic is being published (`#559 `_) +* Fixed cpplint on xenial (`#556 `_) +* Added get_parameter_or_set_default. (`#551 `_) +* Added max_duration to spin_some() (`#558 `_) +* Updated to output rcl error message when yaml parsing fails (`#557 `_) +* Updated to make sure timer is fini'd before clock (`#553 `_) +* Get node names and namespaces (`#545 `_) +* Fixed and improved documentation (`#546 `_) +* Updated to use rcl_clock_t jump callbacks (`#543 `_) +* Updated to use rcl consolidated wait set functions (`#540 `_) +* Addeed TIME_MAX and DURATION_MAX functions (`#538 `_) +* Updated to publish shared_ptr of rcl_serialized_message (`#541 `_) +* Added Time::is_zero and Duration::seconds (`#536 `_) +* Changed to log an error message instead of throwing exception in destructor (`#535 `_) +* Updated to relax tolerance of now test because timing affected by OS scheduling (`#533 `_) +* Removed incorrect exception on sec < 0 (`#527 `_) +* Added rclcpp::Time::seconds() (`#526 `_) +* Updated Timer API to construct TimerBase/GenericTimer with Clock (`#523 `_) +* Added rclcpp::is_initialized() (`#522 `_) +* Added support for jump handlers with only pre- or post-jump callback (`#517 `_) +* Removed use of uninitialized CMake var (`#512 `_) +* Updated for Uncrustify 0.67 (`#510 `_) +* Added get_node_names API from node. (`#508 `_) +* Contributors: Anis Ladram, Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Michael Carroll, Mikael Arguedas, Sagnik Basu, Shane Loretz, Sriram Raghunathan, William Woodall, chapulina, dhood + +0.5.0 (2018-06-25) +------------------ +* Fixed a bug in the multi-threaded executor which could cause it to take a timer (potentially other types of wait-able items) more than once to be worked one. (`#383 `_) + * Specifically this could result in a timer getting called more often that it should when using the multi-threaded executor. +* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 `_) +* Changed code to always get the Service name from ``rcl`` to ensure the remapped name is returned. (`#498 `_) +* Added previously missing ``set_parameters_atomically()`` method to the Service client interface. (`#494 `_) +* Added ability to initialize parameter values in a Node via a YAML file passed on the command line. (`#488 `_) +* Fixed the ROS parameter interface which got parameters that aren't set. (`#493 `_) +* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 `_) +* Added a ``Subscription`` tests which uses ``std::bind`` to a class member callback. (`#480 `_) +* Refactored the ``ParameterVariant`` class into the ``Parameter`` and ``ParameterValue`` classes. (`#481 `_) +* Relaxed template matching rules for ``std::bind`` and ``GNU C++ >= 7.1``. (`#484 `_) +* Changed to use the new ``rosgraph_msgs/Clock`` message type for the ``/clock`` topic. (`#474 `_) +* Fixed a flaky ROS time test due to not spinning before getting the time. (`#483 `_) +* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 `_) +* Added support for arrays in Parameters. (`#443 `_) +* Changed how executors use ``AnyExecutable`` objects so that they are a reference instead of a shared pointer, in order to avoid memory allocation in the "common case". (`#463 `_) +* Added ability to pass command line arguments to the Node constructor. (`#461 `_) +* Added an argument to specify the number of threads a multithreaded executor should create. (`#442 `_) +* Changed library export order for static linking. (`#446 `_) +* Fixed some typos in the time unit tests. (`#453 `_) + Obviously it mean RCL_SYSTEM_TIME but not RCL_ROS_TIME in some test cases + * Signed-off-by: jwang +* Added the scale operation to ``rclcpp::Duration``. + * Signed-off-by: jwang +* Changed API of the log location parameter to be ``const``. (`#451 `_) +* Changed how the subscriber, client, service, and timer handles are stored to resolve shutdown order issues. (`#431 `_ and `#448 `_) +* Updated to get the node's logger name from ``rcl``. (`#433 `_) +* Now depends on ``ament_cmake_ros``. (`#444 `_) +* Updaed code to use logging macros rather than ``fprintf()``. (`#439 `_) +* Fixed a bug that was using an invalid iterator when erasing items using an iterator in a loop. (`#436 `_) +* Changed code to support move of ``rcutils_time_point_value_t`` type from ``uint64_t`` to ``int64_t``. (`#429 `_) +* Renamed parameter byte type to ``byte_values`` from ``bytes_value``. (`#428 `_) +* Changed executor code to clear the wait set before resizing and waiting. (`#427 `_) +* Fixed a potential dereference of nullptr in the topic name validation error string. (`#405 `_) + * Signed-off-by: Ethan Gao +* Changed to use ``rcl_count_publishers()`` like API's rather than the lower level ``rmw_count_publishers()`` API. (`#425 `_) + * Signed-off-by: Sriram Raghunathan +* Fix potential segmentation fault due to ``get_topic_name()`` or ``rcl_service_get_service_name()`` returning nullptr and that not being checked before access in ``rclcpp``. (`#426 `_) + * Signed-off-by: Ethan Gao +* Contributors: Denise Eng, Dirk Thomas, Ernesto Corbellini, Esteve Fernandez, Ethan Gao, Guillaume Autran, Karsten Knese, Matthew, Michael Carroll, Mikael Arguedas, Shane Loretz, Sriram Raghunathan, Tom Moore, William Woodall, dhood, jwang, jwang11, serge-nikulin diff --git a/rclcpp-eloquent/rclcpp/CMakeLists.txt b/rclcpp-eloquent/rclcpp/CMakeLists.txt new file mode 100644 index 0000000..81543dc --- /dev/null +++ b/rclcpp-eloquent/rclcpp/CMakeLists.txt @@ -0,0 +1,505 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp) + +find_package(ament_cmake_ros REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rcl REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(rcl_yaml_param_parser REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rmw REQUIRED) +find_package(rmw_implementation REQUIRED) +find_package(rosgraph_msgs REQUIRED) +find_package(rosidl_generator_cpp REQUIRED) +find_package(rosidl_typesupport_c REQUIRED) +find_package(rosidl_typesupport_cpp REQUIRED) +find_package(tracetools REQUIRED) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories(include) + +set(${PROJECT_NAME}_SRCS + src/rclcpp/any_executable.cpp + src/rclcpp/callback_group.cpp + src/rclcpp/client.cpp + src/rclcpp/clock.cpp + src/rclcpp/context.cpp + src/rclcpp/contexts/default_context.cpp + src/rclcpp/detail/rmw_implementation_specific_payload.cpp + src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp + src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp + src/rclcpp/duration.cpp + src/rclcpp/event.cpp + src/rclcpp/exceptions.cpp + src/rclcpp/executor.cpp + src/rclcpp/executors.cpp + src/rclcpp/expand_topic_or_service_name.cpp + src/rclcpp/executors/multi_threaded_executor.cpp + src/rclcpp/executors/single_threaded_executor.cpp + src/rclcpp/graph_listener.cpp + src/rclcpp/init_options.cpp + src/rclcpp/intra_process_manager.cpp + src/rclcpp/logger.cpp + src/rclcpp/memory_strategies.cpp + src/rclcpp/memory_strategy.cpp + src/rclcpp/node.cpp + src/rclcpp/node_options.cpp + src/rclcpp/node_interfaces/node_base.cpp + src/rclcpp/node_interfaces/node_clock.cpp + src/rclcpp/node_interfaces/node_graph.cpp + src/rclcpp/node_interfaces/node_logging.cpp + src/rclcpp/node_interfaces/node_parameters.cpp + src/rclcpp/node_interfaces/node_services.cpp + src/rclcpp/node_interfaces/node_time_source.cpp + src/rclcpp/node_interfaces/node_timers.cpp + src/rclcpp/node_interfaces/node_topics.cpp + src/rclcpp/node_interfaces/node_waitables.cpp + src/rclcpp/parameter.cpp + src/rclcpp/parameter_value.cpp + src/rclcpp/parameter_client.cpp + src/rclcpp/parameter_events_filter.cpp + src/rclcpp/parameter_map.cpp + src/rclcpp/parameter_service.cpp + src/rclcpp/publisher_base.cpp + src/rclcpp/qos.cpp + src/rclcpp/qos_event.cpp + src/rclcpp/service.cpp + src/rclcpp/signal_handler.cpp + src/rclcpp/subscription_base.cpp + src/rclcpp/subscription_intra_process_base.cpp + src/rclcpp/time.cpp + src/rclcpp/time_source.cpp + src/rclcpp/timer.cpp + src/rclcpp/type_support.cpp + src/rclcpp/utilities.cpp + src/rclcpp/waitable.cpp +) + +# "watch" template for changes +configure_file( + "resource/logging.hpp.em" + "logging.hpp.em.watch" + COPYONLY +) +# generate header with logging macros +set(python_code + "import em" + "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])") +string(REPLACE ";" "$" python_code "${python_code}") +add_custom_command(OUTPUT include/rclcpp/logging.hpp + COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp" + COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}" + DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch" + COMMENT "Expanding logging.hpp.em" + VERBATIM +) +list(APPEND ${PROJECT_NAME}_SRCS + include/rclcpp/logging.hpp) +include_directories("${CMAKE_CURRENT_BINARY_DIR}/include") + +add_library(${PROJECT_NAME} + ${${PROJECT_NAME}_SRCS}) +# specific order: dependents before dependencies +ament_target_dependencies(${PROJECT_NAME} + "rcl" + "rcl_yaml_param_parser" + "rcpputils" + "builtin_interfaces" + "rosgraph_msgs" + "rosidl_typesupport_cpp" + "rosidl_generator_cpp" + "tracetools" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} + PRIVATE "RCLCPP_BUILDING_LIBRARY") + +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# specific order: dependents before dependencies +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_export_dependencies(ament_cmake) +ament_export_dependencies(rcl) +ament_export_dependencies(rcpputils) +ament_export_dependencies(builtin_interfaces) +ament_export_dependencies(rosgraph_msgs) +ament_export_dependencies(rosidl_typesupport_cpp) +ament_export_dependencies(rosidl_typesupport_c) +ament_export_dependencies(rosidl_generator_cpp) +ament_export_dependencies(rcl_yaml_param_parser) +ament_export_dependencies(tracetools) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(rmw_implementation_cmake REQUIRED) + + find_package(test_msgs REQUIRED) + + include(cmake/rclcpp_add_build_failure_test.cmake) + + add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources") + + ament_add_gtest(test_client test/test_client.cpp) + if(TARGET test_client) + ament_target_dependencies(test_client + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_client ${PROJECT_NAME}) + endif() + ament_add_gtest(test_create_timer test/test_create_timer.cpp) + if(TARGET test_create_timer) + ament_target_dependencies(test_create_timer + "rcl_interfaces" + "rmw" + "rcl" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_create_timer ${PROJECT_NAME}) + target_include_directories(test_create_timer PRIVATE test/) + endif() + ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp) + if(TARGET test_expand_topic_or_service_name) + ament_target_dependencies(test_expand_topic_or_service_name + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME}) + endif() + ament_add_gtest(test_function_traits test/test_function_traits.cpp) + if(TARGET test_function_traits) + ament_target_dependencies(test_function_traits + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + endif() + ament_add_gmock(test_intra_process_manager test/test_intra_process_manager.cpp) + if(TARGET test_intra_process_manager) + ament_target_dependencies(test_intra_process_manager + "rcl" + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) + endif() + ament_add_gtest(test_ring_buffer_implementation test/test_ring_buffer_implementation.cpp) + if(TARGET test_ring_buffer_implementation) + ament_target_dependencies(test_ring_buffer_implementation + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) + endif() + ament_add_gtest(test_intra_process_buffer test/test_intra_process_buffer.cpp) + if(TARGET test_intra_process_buffer) + ament_target_dependencies(test_intra_process_buffer + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_loaned_message test/test_loaned_message.cpp) + ament_target_dependencies(test_loaned_message + "test_msgs" + ) + target_link_libraries(test_loaned_message ${PROJECT_NAME}) + + ament_add_gtest(test_node test/test_node.cpp TIMEOUT 240) + if(TARGET test_node) + ament_target_dependencies(test_node + "rcl_interfaces" + "rcpputils" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_node ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_node_interfaces__get_node_interfaces + test/node_interfaces/test_get_node_interfaces.cpp) + if(TARGET test_node_interfaces__get_node_interfaces) + target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) + endif() + + # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node + # ${PROJECT_NAME}) + + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node + # ${PROJECT_NAME}) + + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node + # ${PROJECT_NAME}) + + # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node + # test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp) + # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node + # ${PROJECT_NAME}) + + ament_add_gtest(test_node_global_args test/test_node_global_args.cpp) + if(TARGET test_node_global_args) + ament_target_dependencies(test_node_global_args + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_node_global_args ${PROJECT_NAME}) + endif() + ament_add_gtest(test_node_options test/test_node_options.cpp) + if(TARGET test_node_options) + ament_target_dependencies(test_node_options "rcl") + target_link_libraries(test_node_options ${PROJECT_NAME}) + endif() + ament_add_gtest(test_parameter_client test/test_parameter_client.cpp) + if(TARGET test_parameter_client) + ament_target_dependencies(test_parameter_client + "rcl_interfaces" + ) + target_link_libraries(test_parameter_client ${PROJECT_NAME}) + endif() + ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp) + if(TARGET test_parameter_events_filter) + ament_target_dependencies(test_parameter_events_filter + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) + endif() + ament_add_gtest(test_parameter test/test_parameter.cpp) + if(TARGET test_parameter) + ament_target_dependencies(test_parameter + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter ${PROJECT_NAME}) + endif() + ament_add_gtest(test_parameter_map test/test_parameter_map.cpp) + if(TARGET test_parameter_map) + target_link_libraries(test_parameter_map ${PROJECT_NAME}) + endif() + ament_add_gtest(test_publisher test/test_publisher.cpp) + if(TARGET test_publisher) + ament_target_dependencies(test_publisher + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_publisher ${PROJECT_NAME}) + endif() + ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp) + if(TARGET test_publisher_subscription_count_api) + ament_target_dependencies(test_publisher_subscription_count_api + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) + endif() + ament_add_gtest(test_rate test/test_rate.cpp) + if(TARGET test_rate) + ament_target_dependencies(test_rate + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_rate + ${PROJECT_NAME} + ) + endif() + ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp) + if(TARGET test_serialized_message_allocator) + ament_target_dependencies(test_serialized_message_allocator + test_msgs + ) + target_link_libraries(test_serialized_message_allocator + ${PROJECT_NAME} + ) + endif() + ament_add_gtest(test_service test/test_service.cpp) + if(TARGET test_service) + ament_target_dependencies(test_service + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_service ${PROJECT_NAME}) + endif() + ament_add_gtest(test_subscription test/test_subscription.cpp) + if(TARGET test_subscription) + ament_target_dependencies(test_subscription + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_subscription ${PROJECT_NAME}) + endif() + ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp) + if(TARGET test_subscription_publisher_count_api) + ament_target_dependencies(test_subscription_publisher_count_api + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) + endif() + ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp) + if(TARGET test_subscription_traits) + ament_target_dependencies(test_subscription_traits + "rcl" + "test_msgs" + ) + endif() + ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) + if(TARGET test_find_weak_nodes) + ament_target_dependencies(test_find_weak_nodes + "rcl" + ) + target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) + endif() + + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") + if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") + endif() + + ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp) + ament_target_dependencies(test_externally_defined_services + "rcl" + "test_msgs" + ) + target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) + + ament_add_gtest(test_duration test/test_duration.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_duration) + ament_target_dependencies(test_duration + "rcl") + target_link_libraries(test_duration ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_executor test/test_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_executor) + ament_target_dependencies(test_executor + "rcl") + target_link_libraries(test_executor ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_logger test/test_logger.cpp) + target_link_libraries(test_logger ${PROJECT_NAME}) + + ament_add_gmock(test_logging test/test_logging.cpp) + target_link_libraries(test_logging ${PROJECT_NAME}) + + ament_add_gtest(test_time test/test_time.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_time) + ament_target_dependencies(test_time + "rcl") + target_link_libraries(test_time ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_timer test/test_timer.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_timer) + ament_target_dependencies(test_timer + "rcl") + target_link_libraries(test_timer ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_time_source test/test_time_source.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_time_source) + ament_target_dependencies(test_time_source + "rcl") + target_link_libraries(test_time_source ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_utilities test/test_utilities.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_utilities) + ament_target_dependencies(test_utilities + "rcl") + target_link_libraries(test_utilities ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_init test/test_init.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_init) + ament_target_dependencies(test_init + "rcl") + target_link_libraries(test_init ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_multi_threaded_executor) + ament_target_dependencies(test_multi_threaded_executor + "rcl") + target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) + endif() + + # Install test resources + install( + DIRECTORY test/resources + DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test) +endif() + +ament_package() + +install( + DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/ + DESTINATION include +) diff --git a/rclcpp-eloquent/rclcpp/Doxyfile b/rclcpp-eloquent/rclcpp/Doxyfile new file mode 100644 index 0000000..2a7846b --- /dev/null +++ b/rclcpp-eloquent/rclcpp/Doxyfile @@ -0,0 +1,33 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp" +PROJECT_NUMBER = master +PROJECT_BRIEF = "C++ ROS Client Library API" + +# Use these lines to include the generated logging.hpp (update install path if needed) +#INPUT = ../../../../install_isolated/rclcpp/include +#STRIP_FROM_PATH = /Users/william/ros2_ws/install_isolated/rclcpp/include +# Otherwise just generate for the local (non-generated header files) +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +# Uncomment to generate tag files for cross-project linking. +#GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp.tag" diff --git a/rclcpp-eloquent/rclcpp/cmake/rclcpp_add_build_failure_test.cmake b/rclcpp-eloquent/rclcpp/cmake/rclcpp_add_build_failure_test.cmake new file mode 100644 index 0000000..5ccde6e --- /dev/null +++ b/rclcpp-eloquent/rclcpp/cmake/rclcpp_add_build_failure_test.cmake @@ -0,0 +1,56 @@ +# Copyright 2019 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. + +# +# Register a test which tries to compile a file and expects it to fail to build. +# +# This will create two targets, one by the given target name and a test target +# which has the same name prefixed with `test_`. +# For example, if target is `should_not_compile__use_const_argument` then there +# will be an executable target called `should_not_compile__use_const_argument` +# and a test target called `test_should_not_compile__use_const_argument`. +# +# :param target: the name of the target to be created +# :type target: string +# :param ARGN: the list of source files to be used to create the test executable +# :type ARGN: list of strings +# +macro(rclcpp_add_build_failure_test target) + if(${ARGC} EQUAL 0) + message( + FATAL_ERROR + "rclcpp_add_build_failure_test() requires a target name and " + "at least one source file") + endif() + + add_executable(${target} ${ARGN}) + set_target_properties(${target} + PROPERTIES + EXCLUDE_FROM_ALL TRUE + EXCLUDE_FROM_DEFAULT_BUILD TRUE) + + add_test( + NAME test_${target} + COMMAND + ${CMAKE_COMMAND} + --build . + --target ${target} + --config $ + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) + set_tests_properties(test_${target} + PROPERTIES + WILL_FAIL TRUE + LABELS "build_failure" + ) +endmacro() diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_common.hpp new file mode 100644 index 0000000..d117376 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -0,0 +1,99 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ +#define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ + +#include + +#include "rcl/allocator.h" + +#include "rclcpp/allocator/allocator_deleter.hpp" + +namespace rclcpp +{ +namespace allocator +{ + +template +using AllocRebind = typename std::allocator_traits::template rebind_traits; + +template +void * retyped_allocate(size_t size, void * untyped_allocator) +{ + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + return std::allocator_traits::allocate(*typed_allocator, size); +} + +template +void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) +{ + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + auto typed_ptr = static_cast(untyped_pointer); + std::allocator_traits::deallocate(*typed_allocator, typed_ptr, 1); +} + +template +void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_allocator) +{ + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + auto typed_ptr = static_cast(untyped_pointer); + std::allocator_traits::deallocate(*typed_allocator, typed_ptr, 1); + return std::allocator_traits::allocate(*typed_allocator, size); +} + + +// Convert a std::allocator_traits-formatted Allocator into an rcl allocator +template< + typename T, + typename Alloc, + typename std::enable_if>::value>::type * = nullptr> +rcl_allocator_t get_rcl_allocator(Alloc & allocator) +{ + rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); +#ifndef _WIN32 + rcl_allocator.allocate = &retyped_allocate; + rcl_allocator.deallocate = &retyped_deallocate; + rcl_allocator.reallocate = &retyped_reallocate; + rcl_allocator.state = &allocator; +#else + (void)allocator; // Remove warning +#endif + return rcl_allocator; +} + +// TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator +template< + typename T, + typename Alloc, + typename std::enable_if>::value>::type * = nullptr> +rcl_allocator_t get_rcl_allocator(Alloc & allocator) +{ + (void)allocator; + return rcl_get_default_allocator(); +} + +} // namespace allocator +} // namespace rclcpp + +#endif // RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp new file mode 100644 index 0000000..8e44ea8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -0,0 +1,105 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_ +#define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_ + +#include +#include + +namespace rclcpp +{ +namespace allocator +{ + +template +class AllocatorDeleter +{ + template + using AllocRebind = typename std::allocator_traits::template rebind_alloc; + +public: + AllocatorDeleter() + : allocator_(nullptr) + { + } + + explicit AllocatorDeleter(Allocator * a) + : allocator_(a) + { + } + + template + AllocatorDeleter(const AllocatorDeleter & a) + { + allocator_ = a.get_allocator(); + } + + template + void operator()(T * ptr) + { + std::allocator_traits>::destroy(*allocator_, ptr); + std::allocator_traits>::deallocate(*allocator_, ptr, 1); + ptr = nullptr; + } + + Allocator * get_allocator() const + { + return allocator_; + } + + void set_allocator(Allocator * alloc) + { + allocator_ = alloc; + } + +private: + Allocator * allocator_; +}; + +template +void set_allocator_for_deleter(D * deleter, Alloc * alloc) +{ + (void) alloc; + (void) deleter; + throw std::runtime_error("Reached unexpected template specialization"); +} + +template +void set_allocator_for_deleter(std::default_delete * deleter, std::allocator * alloc) +{ + (void) deleter; + (void) alloc; +} + +template +void set_allocator_for_deleter(AllocatorDeleter * deleter, Alloc * alloc) +{ + if (!deleter || !alloc) { + throw std::invalid_argument("Argument was NULL to set_allocator_for_deleter"); + } + deleter->set_allocator(alloc); +} + +template +using Deleter = typename std::conditional< + std::is_same::template rebind_alloc, + typename std::allocator::template rebind::other>::value, + std::default_delete, + AllocatorDeleter + >::type; +} // namespace allocator +} // namespace rclcpp + +#endif // RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/any_executable.hpp new file mode 100644 index 0000000..aae25dc --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/any_executable.hpp @@ -0,0 +1,57 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__ANY_EXECUTABLE_HPP_ +#define RCLCPP__ANY_EXECUTABLE_HPP_ + +#include + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace executor +{ + +struct AnyExecutable +{ + RCLCPP_PUBLIC + AnyExecutable(); + + RCLCPP_PUBLIC + virtual ~AnyExecutable(); + + // Only one of the following pointers will be set. + rclcpp::SubscriptionBase::SharedPtr subscription; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::ServiceBase::SharedPtr service; + rclcpp::ClientBase::SharedPtr client; + rclcpp::Waitable::SharedPtr waitable; + // These are used to keep the scope on the containing items + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; +}; + +} // namespace executor +} // namespace rclcpp + +#endif // RCLCPP__ANY_EXECUTABLE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/any_service_callback.hpp new file mode 100644 index 0000000..294d2f2 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/any_service_callback.hpp @@ -0,0 +1,121 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_ +#define RCLCPP__ANY_SERVICE_CALLBACK_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/function_traits.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/types.h" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" + +namespace rclcpp +{ + +template +class AnyServiceCallback +{ +private: + using SharedPtrCallback = std::function< + void ( + const std::shared_ptr, + std::shared_ptr + )>; + using SharedPtrWithRequestHeaderCallback = std::function< + void ( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr + )>; + + SharedPtrCallback shared_ptr_callback_; + SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_; + +public: + AnyServiceCallback() + : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr) + {} + + AnyServiceCallback(const AnyServiceCallback &) = default; + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + shared_ptr_callback_ = callback; + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + shared_ptr_with_request_header_callback_ = callback; + } + + void dispatch( + std::shared_ptr request_header, + std::shared_ptr request, + std::shared_ptr response) + { + TRACEPOINT(callback_start, (const void *)this, false); + if (shared_ptr_callback_ != nullptr) { + (void)request_header; + shared_ptr_callback_(request, response); + } else if (shared_ptr_with_request_header_callback_ != nullptr) { + shared_ptr_with_request_header_callback_(request_header, request, response); + } else { + throw std::runtime_error("unexpected request without any callback set"); + } + TRACEPOINT(callback_end, (const void *)this); + } + + void register_callback_for_tracing() + { + if (shared_ptr_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(shared_ptr_callback_)); + } else if (shared_ptr_with_request_header_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(shared_ptr_with_request_header_callback_)); + } + } +}; + +} // namespace rclcpp + +#endif // RCLCPP__ANY_SERVICE_CALLBACK_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/any_subscription_callback.hpp new file mode 100644 index 0000000..f6b669f --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -0,0 +1,267 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ +#define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/function_traits.hpp" +#include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" + +namespace rclcpp +{ + +template +class AnySubscriptionCallback +{ + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using SharedPtrCallback = std::function)>; + using SharedPtrWithInfoCallback = + std::function, const rmw_message_info_t &)>; + using ConstSharedPtrCallback = std::function)>; + using ConstSharedPtrWithInfoCallback = + std::function, const rmw_message_info_t &)>; + using UniquePtrCallback = std::function; + using UniquePtrWithInfoCallback = + std::function; + + SharedPtrCallback shared_ptr_callback_; + SharedPtrWithInfoCallback shared_ptr_with_info_callback_; + ConstSharedPtrCallback const_shared_ptr_callback_; + ConstSharedPtrWithInfoCallback const_shared_ptr_with_info_callback_; + UniquePtrCallback unique_ptr_callback_; + UniquePtrWithInfoCallback unique_ptr_with_info_callback_; + +public: + explicit AnySubscriptionCallback(std::shared_ptr allocator) + : shared_ptr_callback_(nullptr), shared_ptr_with_info_callback_(nullptr), + const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr), + unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr) + { + message_allocator_ = std::make_shared(*allocator.get()); + allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + } + + AnySubscriptionCallback(const AnySubscriptionCallback &) = default; + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + shared_ptr_callback_ = callback; + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithInfoCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + shared_ptr_with_info_callback_ = callback; + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + ConstSharedPtrCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + const_shared_ptr_callback_ = callback; + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + ConstSharedPtrWithInfoCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + const_shared_ptr_with_info_callback_ = callback; + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + UniquePtrCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + unique_ptr_callback_ = callback; + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + UniquePtrWithInfoCallback + >::value + >::type * = nullptr + > + void set(CallbackT callback) + { + unique_ptr_with_info_callback_ = callback; + } + + void dispatch( + std::shared_ptr message, const rmw_message_info_t & message_info) + { + TRACEPOINT(callback_start, (const void *)this, false); + if (shared_ptr_callback_) { + shared_ptr_callback_(message); + } else if (shared_ptr_with_info_callback_) { + shared_ptr_with_info_callback_(message, message_info); + } else if (const_shared_ptr_callback_) { + const_shared_ptr_callback_(message); + } else if (const_shared_ptr_with_info_callback_) { + const_shared_ptr_with_info_callback_(message, message_info); + } else if (unique_ptr_callback_) { + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); + unique_ptr_callback_(MessageUniquePtr(ptr, message_deleter_)); + } else if (unique_ptr_with_info_callback_) { + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); + unique_ptr_with_info_callback_(MessageUniquePtr(ptr, message_deleter_), message_info); + } else { + throw std::runtime_error("unexpected message without any callback set"); + } + TRACEPOINT(callback_end, (const void *)this); + } + + void dispatch_intra_process( + ConstMessageSharedPtr message, const rmw_message_info_t & message_info) + { + TRACEPOINT(callback_start, (const void *)this, true); + if (const_shared_ptr_callback_) { + const_shared_ptr_callback_(message); + } else if (const_shared_ptr_with_info_callback_) { + const_shared_ptr_with_info_callback_(message, message_info); + } else { + if ( + unique_ptr_callback_ || unique_ptr_with_info_callback_ || + shared_ptr_callback_ || shared_ptr_with_info_callback_) + { + throw std::runtime_error( + "unexpected dispatch_intra_process const shared " + "message call with no const shared_ptr callback"); + } else { + throw std::runtime_error("unexpected message without any callback set"); + } + } + TRACEPOINT(callback_end, (const void *)this); + } + + void dispatch_intra_process( + MessageUniquePtr message, const rmw_message_info_t & message_info) + { + TRACEPOINT(callback_start, (const void *)this, true); + if (shared_ptr_callback_) { + typename std::shared_ptr shared_message = std::move(message); + shared_ptr_callback_(shared_message); + } else if (shared_ptr_with_info_callback_) { + typename std::shared_ptr shared_message = std::move(message); + shared_ptr_with_info_callback_(shared_message, message_info); + } else if (unique_ptr_callback_) { + unique_ptr_callback_(std::move(message)); + } else if (unique_ptr_with_info_callback_) { + unique_ptr_with_info_callback_(std::move(message), message_info); + } else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_) { + throw std::runtime_error( + "unexpected dispatch_intra_process unique message call" + " with const shared_ptr callback"); + } else { + throw std::runtime_error("unexpected message without any callback set"); + } + TRACEPOINT(callback_end, (const void *)this); + } + + bool use_take_shared_method() const + { + return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_; + } + + void register_callback_for_tracing() + { + if (shared_ptr_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(shared_ptr_callback_)); + } else if (shared_ptr_with_info_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(shared_ptr_with_info_callback_)); + } else if (unique_ptr_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(unique_ptr_callback_)); + } else if (unique_ptr_with_info_callback_) { + TRACEPOINT( + rclcpp_callback_register, + (const void *)this, + get_symbol(unique_ptr_with_info_callback_)); + } + } + +private: + std::shared_ptr message_allocator_; + MessageDeleter message_deleter_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/callback_group.hpp new file mode 100644 index 0000000..ef87a2f --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/callback_group.hpp @@ -0,0 +1,168 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CALLBACK_GROUP_HPP_ +#define RCLCPP__CALLBACK_GROUP_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/client.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ + +// Forward declarations for friend statement in class CallbackGroup +namespace node_interfaces +{ +class NodeServices; +class NodeTimers; +class NodeTopics; +class NodeWaitables; +} // namespace node_interfaces + +namespace callback_group +{ + +enum class CallbackGroupType +{ + MutuallyExclusive, + Reentrant +}; + +class CallbackGroup +{ + friend class rclcpp::node_interfaces::NodeServices; + friend class rclcpp::node_interfaces::NodeTimers; + friend class rclcpp::node_interfaces::NodeTopics; + friend class rclcpp::node_interfaces::NodeWaitables; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup) + + RCLCPP_PUBLIC + explicit CallbackGroup(CallbackGroupType group_type); + + template + rclcpp::SubscriptionBase::SharedPtr + find_subscription_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, subscription_ptrs_); + } + + template + rclcpp::TimerBase::SharedPtr + find_timer_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, timer_ptrs_); + } + + template + rclcpp::ServiceBase::SharedPtr + find_service_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, service_ptrs_); + } + + template + rclcpp::ClientBase::SharedPtr + find_client_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, client_ptrs_); + } + + template + rclcpp::Waitable::SharedPtr + find_waitable_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, waitable_ptrs_); + } + + RCLCPP_PUBLIC + std::atomic_bool & + can_be_taken_from(); + + RCLCPP_PUBLIC + const CallbackGroupType & + type() const; + +protected: + RCLCPP_DISABLE_COPY(CallbackGroup) + + RCLCPP_PUBLIC + void + add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr); + + RCLCPP_PUBLIC + void + add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr); + + RCLCPP_PUBLIC + void + add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr); + + RCLCPP_PUBLIC + void + add_service(const rclcpp::ServiceBase::SharedPtr service_ptr); + + RCLCPP_PUBLIC + void + add_client(const rclcpp::ClientBase::SharedPtr client_ptr); + + RCLCPP_PUBLIC + void + add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr); + + RCLCPP_PUBLIC + void + remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept; + + CallbackGroupType type_; + // Mutex to protect the subsequent vectors of pointers. + mutable std::mutex mutex_; + std::vector subscription_ptrs_; + std::vector timer_ptrs_; + std::vector service_ptrs_; + std::vector client_ptrs_; + std::vector waitable_ptrs_; + std::atomic_bool can_be_taken_from_; + +private: + template + typename TypeT::SharedPtr _find_ptrs_if_impl( + Function func, const std::vector & vect_ptrs) const + { + std::lock_guard lock(mutex_); + for (auto & weak_ptr : vect_ptrs) { + auto ref_ptr = weak_ptr.lock(); + if (ref_ptr && func(ref_ptr)) { + return ref_ptr; + } + } + return typename TypeT::SharedPtr(); + } +}; + +} // namespace callback_group +} // namespace rclcpp + +#endif // RCLCPP__CALLBACK_GROUP_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/client.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/client.hpp new file mode 100644 index 0000000..4646740 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/client.hpp @@ -0,0 +1,282 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CLIENT_HPP_ +#define RCLCPP__CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/client.h" +#include "rcl/error_handling.h" +#include "rcl/wait.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/function_traits.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcutils/logging_macros.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +namespace node_interfaces +{ +class NodeBaseInterface; +} // namespace node_interfaces + +class ClientBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase) + + RCLCPP_PUBLIC + ClientBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph); + + RCLCPP_PUBLIC + virtual ~ClientBase(); + + RCLCPP_PUBLIC + const char * + get_service_name() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_client_handle(); + + RCLCPP_PUBLIC + std::shared_ptr + get_client_handle() const; + + RCLCPP_PUBLIC + bool + service_is_ready() const; + + template + bool + wait_for_service( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return wait_for_service_nanoseconds( + std::chrono::duration_cast(timeout) + ); + } + + virtual std::shared_ptr create_response() = 0; + virtual std::shared_ptr create_request_header() = 0; + virtual void handle_response( + std::shared_ptr request_header, std::shared_ptr response) = 0; + +protected: + RCLCPP_DISABLE_COPY(ClientBase) + + RCLCPP_PUBLIC + bool + wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + rcl_node_t * + get_rcl_node_handle(); + + RCLCPP_PUBLIC + const rcl_node_t * + get_rcl_node_handle() const; + + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; + std::shared_ptr node_handle_; + std::shared_ptr context_; + + std::shared_ptr client_handle_; +}; + +template +class Client : public ClientBase +{ +public: + using SharedRequest = typename ServiceT::Request::SharedPtr; + using SharedResponse = typename ServiceT::Response::SharedPtr; + + using Promise = std::promise; + using PromiseWithRequest = std::promise>; + + using SharedPromise = std::shared_ptr; + using SharedPromiseWithRequest = std::shared_ptr; + + using SharedFuture = std::shared_future; + using SharedFutureWithRequest = std::shared_future>; + + using CallbackType = std::function; + using CallbackWithRequestType = std::function; + + RCLCPP_SMART_PTR_DEFINITIONS(Client) + + Client( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + const std::string & service_name, + rcl_client_options_t & client_options) + : ClientBase(node_base, node_graph) + { + using rosidl_typesupport_cpp::get_service_type_support_handle; + auto service_type_support_handle = + get_service_type_support_handle(); + rcl_ret_t ret = rcl_client_init( + this->get_client_handle().get(), + this->get_rcl_node_handle(), + service_type_support_handle, + service_name.c_str(), + &client_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = this->get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client"); + } + } + + virtual ~Client() + { + } + + std::shared_ptr + create_response() override + { + return std::shared_ptr(new typename ServiceT::Response()); + } + + std::shared_ptr + create_request_header() override + { + // TODO(wjwwood): This should probably use rmw_request_id's allocator. + // (since it is a C type) + return std::shared_ptr(new rmw_request_id_t); + } + + void + handle_response( + std::shared_ptr request_header, + std::shared_ptr response) override + { + std::unique_lock lock(pending_requests_mutex_); + auto typed_response = std::static_pointer_cast(response); + int64_t sequence_number = request_header->sequence_number; + // TODO(esteve) this should throw instead since it is not expected to happen in the first place + if (this->pending_requests_.count(sequence_number) == 0) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Received invalid sequence number. Ignoring..."); + return; + } + auto tuple = this->pending_requests_[sequence_number]; + auto call_promise = std::get<0>(tuple); + auto callback = std::get<1>(tuple); + auto future = std::get<2>(tuple); + this->pending_requests_.erase(sequence_number); + // Unlock here to allow the service to be called recursively from one of its callbacks. + lock.unlock(); + + call_promise->set_value(typed_response); + callback(future); + } + + SharedFuture + async_send_request(SharedRequest request) + { + return async_send_request(request, [](SharedFuture) {}); + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + CallbackType + >::value + >::type * = nullptr + > + SharedFuture + async_send_request(SharedRequest request, CallbackT && cb) + { + std::lock_guard lock(pending_requests_mutex_); + int64_t sequence_number; + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); + } + + SharedPromise call_promise = std::make_shared(); + SharedFuture f(call_promise->get_future()); + pending_requests_[sequence_number] = + std::make_tuple(call_promise, std::forward(cb), f); + return f; + } + + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + CallbackWithRequestType + >::value + >::type * = nullptr + > + SharedFutureWithRequest + async_send_request(SharedRequest request, CallbackT && cb) + { + SharedPromiseWithRequest promise = std::make_shared(); + SharedFutureWithRequest future_with_request(promise->get_future()); + + auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) { + auto response = future.get(); + promise->set_value(std::make_pair(request, response)); + cb(future_with_request); + }; + + async_send_request(request, wrapping_cb); + + return future_with_request; + } + +private: + RCLCPP_DISABLE_COPY(Client) + + std::map> pending_requests_; + std::mutex pending_requests_mutex_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__CLIENT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/clock.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/clock.hpp new file mode 100644 index 0000000..02c6167 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/clock.hpp @@ -0,0 +1,143 @@ +// Copyright 2017 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 RCLCPP__CLOCK_HPP_ +#define RCLCPP__CLOCK_HPP_ + +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl/time.h" +#include "rcutils/time.h" +#include "rcutils/types/rcutils_ret.h" + +namespace rclcpp +{ + +class TimeSource; + +class JumpHandler +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler) + + using pre_callback_t = std::function; + using post_callback_t = std::function; + + JumpHandler( + pre_callback_t pre_callback, + post_callback_t post_callback, + const rcl_jump_threshold_t & threshold); + + pre_callback_t pre_callback; + post_callback_t post_callback; + rcl_jump_threshold_t notice_threshold; +}; + +class Clock +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Clock) + + /// Default c'tor + /** + * Initializes the clock instance with the given clock_type. + * + * \param clock_type type of the clock. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ + RCLCPP_PUBLIC + explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); + + RCLCPP_PUBLIC + ~Clock(); + + /** + * Returns current time from the time source specified by clock_type. + * + * \return current time. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ + RCLCPP_PUBLIC + Time + now(); + + /** + * Returns the clock of the type `RCL_ROS_TIME` is active. + * + * \return true if the clock is active + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if + * the current clock does not have the clock_type `RCL_ROS_TIME`. + */ + RCLCPP_PUBLIC + bool + ros_time_is_active(); + + RCLCPP_PUBLIC + rcl_clock_t * + get_clock_handle() noexcept; + + RCLCPP_PUBLIC + rcl_clock_type_t + get_clock_type() const noexcept; + + // Add a callback to invoke if the jump threshold is exceeded. + /** + * These callback functions must remain valid as long as the + * returned shared pointer is valid. + * + * Function will register callbacks to the callback queue. On time jump all + * callbacks will be executed whose threshold is greater then the time jump; + * The logic will first call selected pre_callbacks and then all selected + * post_callbacks. + * + * Function is only applicable if the clock_type is `RCL_ROS_TIME` + * + * \param pre_callback. Must be non-throwing + * \param post_callback. Must be non-throwing. + * \param threshold. Callbacks will be triggered if the time jump is greater + * then the threshold. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + * \throws std::bad_alloc if the allocation of the JumpHandler fails. + * \warning the instance of the clock must remain valid as long as any created + * JumpHandler. + */ + RCLCPP_PUBLIC + JumpHandler::SharedPtr + create_jump_callback( + JumpHandler::pre_callback_t pre_callback, + JumpHandler::post_callback_t post_callback, + const rcl_jump_threshold_t & threshold); + +private: + // Invoke time jump callback + RCLCPP_PUBLIC + static void + on_time_jump( + const struct rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data); + + /// Internal storage backed by rcl + rcl_clock_t rcl_clock_; + friend TimeSource; /// Allow TimeSource to access the rcl_clock_ datatype. + rcl_allocator_t allocator_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__CLOCK_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/context.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/context.hpp new file mode 100644 index 0000000..dfc33d5 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/context.hpp @@ -0,0 +1,370 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CONTEXT_HPP_ +#define RCLCPP__CONTEXT_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/context.h" +#include "rcl/guard_condition.h" +#include "rcl/wait.h" +#include "rclcpp/init_options.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Thrown when init is called on an already initialized context. +class ContextAlreadyInitialized : public std::runtime_error +{ +public: + ContextAlreadyInitialized() + : std::runtime_error("context is already initialized") {} +}; + +/// Context which encapsulates shared state between nodes and other similar entities. +/** + * A context also represents the lifecycle between init and shutdown of rclcpp. + * It is often used in conjunction with rclcpp::init, or rclcpp::init_local, + * and rclcpp::shutdown. + */ +class Context : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Context) + + /// Default constructor, after which the Context is still not "initialized". + /** + * Every context which is constructed is added to a global vector of contexts, + * which is used by the signal handler to conditionally shutdown each context + * on SIGINT. + * See the shutdown_on_sigint option in the InitOptions class. + */ + RCLCPP_PUBLIC + Context(); + + RCLCPP_PUBLIC + virtual + ~Context(); + + /// Initialize the context, and the underlying elements like the rcl context. + /** + * This method must be called before passing this context to things like the + * constructor of Node. + * It must also be called before trying to shutdown the context. + * + * Note that this function does not setup any signal handlers, so if you want + * it to be shutdown by the signal handler, then you need to either install + * them manually with rclcpp::install_signal_handers() or use rclcpp::init(). + * In addition to installing the signal handlers, the shutdown_on_sigint + * of the InitOptions needs to be `true` for this context to be shutdown by + * the signal handler, otherwise it will be passed over. + * + * After calling this method, shutdown() can be called to invalidate the + * context for derived entities, e.g. nodes, guard conditions, etc. + * However, the underlying rcl context is not finalized until this Context's + * destructor is called or this function is called again. + * Allowing this class to go out of scope and get destructed or calling this + * function a second time while derived entities are still using the context + * is undefined behavior and should be avoided. + * It's a good idea to not reuse context objects and instead create a new one + * each time you need to shutdown and init again. + * This allows derived entities to hold on to shard pointers to the first + * context object until they are done. + * + * This function is thread-safe. + * + * \param[in] argc number of arguments + * \param[in] argv argument array which may contain arguments intended for ROS + * \param[in] init_options initialization options for rclcpp and underlying layers + * \throw ContextAlreadyInitialized if called if init is called more than once + */ + RCLCPP_PUBLIC + virtual + void + init( + int argc, + char const * const argv[], + const rclcpp::InitOptions & init_options = rclcpp::InitOptions()); + + /// Return true if the context is valid, otherwise false. + /** + * The context is valid if it has been initialized but not shutdown. + * + * This function is thread-safe. + * This function is lock free so long as pointers and uint64_t atomics are + * lock free. + * + * \return true if valid, otherwise false + */ + RCLCPP_PUBLIC + bool + is_valid() const; + + /// Return the init options used during init. + RCLCPP_PUBLIC + const rclcpp::InitOptions & + get_init_options() const; + + /// Return a copy of the init options used during init. + RCLCPP_PUBLIC + rclcpp::InitOptions + get_init_options(); + + /// Return the shutdown reason, or empty string if not shutdown. + /** + * This function is thread-safe. + */ + RCLCPP_PUBLIC + std::string + shutdown_reason(); + + /// Shutdown the context, making it uninitialized and therefore invalid for derived entities. + /** + * Several things happen when the context is shutdown, in this order: + * + * - acquires a lock to prevent race conditions with init, on_shutdown, etc. + * - if the context is not initialized, return false + * - rcl_shutdown() is called on the internal rcl_context_t instance + * - the shutdown reason is set + * - each on_shutdown callback is called, in the order that they were added + * - interrupt blocking sleep_for() calls, so they return early due to shutdown + * - interrupt blocking executors and wait sets + * + * The underlying rcl context is not finalized by this function. + * + * This function is thread-safe. + * + * \param[in] reason the description of why shutdown happened + * \return true if shutdown was successful, false if context was already shutdown + * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails + */ + RCLCPP_PUBLIC + virtual + bool + shutdown(const std::string & reason); + + using OnShutdownCallback = std::function; + + /// Add a on_shutdown callback to be called when shutdown is called for this context. + /** + * These callbacks will be called in the order they are added as the second + * to last step in shutdown(). + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronoulsy in the dedicated singal handling thread. + * + * Also, shutdown() may be called from the destructor of this function. + * Therefore, it is not safe to throw exceptions from these callbacks. + * Instead, log errors or use some other mechanism to indicate an error has + * occurred. + * + * On shutdown callbacks may be registered before init and after shutdown, + * and persist on repeated init's. + * + * \param[in] callback the on shutdown callback to be registered + * \return the callback passed, for convenience when storing a passed lambda + */ + RCLCPP_PUBLIC + virtual + OnShutdownCallback + on_shutdown(OnShutdownCallback callback); + + /// Return the shutdown callbacks as const. + /** + * Using the returned reference is not thread-safe with calls that modify + * the list of "on shutdown" callbacks, i.e. on_shutdown(). + */ + RCLCPP_PUBLIC + const std::vector & + get_on_shutdown_callbacks() const; + + /// Return the shutdown callbacks. + /** + * Using the returned reference is not thread-safe with calls that modify + * the list of "on shutdown" callbacks, i.e. on_shutdown(). + */ + RCLCPP_PUBLIC + std::vector & + get_on_shutdown_callbacks(); + + /// Return the internal rcl context. + RCLCPP_PUBLIC + std::shared_ptr + get_rcl_context(); + + /// Sleep for a given period of time or until shutdown() is called. + /** + * This function can be interrupted early if: + * + * - this context is shutdown() + * - this context is destructed (resulting in shutdown) + * - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown) + * - interrupt_all_sleep_for() is called + * + * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. + * \return true if the condition variable did not timeout, i.e. you were interrupted. + */ + RCLCPP_PUBLIC + bool + sleep_for(const std::chrono::nanoseconds & nanoseconds); + + /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true. + RCLCPP_PUBLIC + virtual + void + interrupt_all_sleep_for(); + + /// Get a handle to the guard condition which is triggered when interrupted. + /** + * This guard condition is triggered any time interrupt_all_wait_sets() is + * called, which may be called by the user, or shutdown(). + * And in turn, shutdown() may be called by the user, the destructor of this + * context, or the signal handler if installed and shutdown_on_sigint is true + * for this context. + * + * The first time that this function is called for a given wait set a new guard + * condition will be created and returned; thereafter the same guard condition + * will be returned for the same wait set. + * This mechanism is designed to ensure that the same guard condition is not + * reused across wait sets (e.g., when using multiple executors in the same + * process). + * This method will throw an exception if initialization of the guard + * condition fails. + * + * The returned guard condition needs to be released with the + * release_interrupt_guard_condition() method in order to reclaim resources. + * + * \param[in] wait_set Pointer to the rcl_wait_set_t that will be using the + * resulting guard condition. + * \return Pointer to the guard condition. + */ + RCLCPP_PUBLIC + rcl_guard_condition_t * + get_interrupt_guard_condition(rcl_wait_set_t * wait_set); + + /// Release the previously allocated guard condition which is triggered when interrupted. + /** + * If you previously called get_interrupt_guard_condition() for a given wait + * set to get a interrupt guard condition, then you should call + * release_interrupt_guard_condition() when you're done, to free that + * condition. + * Will throw an exception if get_interrupt_guard_condition() wasn't + * previously called for the given wait set. + * + * After calling this, the pointer returned by get_interrupt_guard_condition() + * for the given wait_set is invalid. + * + * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the + * resulting guard condition. + */ + RCLCPP_PUBLIC + void + release_interrupt_guard_condition(rcl_wait_set_t * wait_set); + + /// Nothrow version of release_interrupt_guard_condition(), logs to RCLCPP_ERROR instead. + RCLCPP_PUBLIC + void + release_interrupt_guard_condition(rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept; + + /// Interrupt any blocking executors, or wait sets associated with this context. + RCLCPP_PUBLIC + virtual + void + interrupt_all_wait_sets(); + + /// Return a singleton instance for the SubContext type, constructing one if necessary. + template + std::shared_ptr + get_sub_context(Args && ... args) + { + std::lock_guard lock(sub_contexts_mutex_); + + std::type_index type_i(typeid(SubContext)); + std::shared_ptr sub_context; + auto it = sub_contexts_.find(type_i); + if (it == sub_contexts_.end()) { + // It doesn't exist yet, make it + sub_context = std::shared_ptr( + new SubContext(std::forward(args) ...), + [](SubContext * sub_context_ptr) { + delete sub_context_ptr; + }); + sub_contexts_[type_i] = sub_context; + } else { + // It exists, get it out and cast it. + sub_context = std::static_pointer_cast(it->second); + } + return sub_context; + } + +protected: + // Called by constructor and destructor to clean up by finalizing the + // shutdown rcl context and preparing for a new init cycle. + RCLCPP_PUBLIC + virtual + void + clean_up(); + +private: + RCLCPP_DISABLE_COPY(Context) + + // This mutex is recursive so that the destructor can ensure atomicity + // between is_initialized and shutdown. + std::recursive_mutex init_mutex_; + std::shared_ptr rcl_context_; + rclcpp::InitOptions init_options_; + std::string shutdown_reason_; + + std::unordered_map> sub_contexts_; + // This mutex is recursive so that the constructor of a sub context may + // attempt to acquire another sub context. + std::recursive_mutex sub_contexts_mutex_; + + std::vector on_shutdown_callbacks_; + std::mutex on_shutdown_callbacks_mutex_; + + /// Condition variable for timed sleep (see sleep_for). + std::condition_variable interrupt_condition_variable_; + /// Mutex for protecting the global condition variable. + std::mutex interrupt_mutex_; + + /// Mutex to protect sigint_guard_cond_handles_. + std::mutex interrupt_guard_cond_handles_mutex_; + /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets(). + std::unordered_map interrupt_guard_cond_handles_; +}; + +/// Return a copy of the list of context shared pointers. +/** + * This function is thread-safe. + */ +RCLCPP_PUBLIC +std::vector +get_contexts(); + +} // namespace rclcpp + +#endif // RCLCPP__CONTEXT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/contexts/default_context.hpp new file mode 100644 index 0000000..2ed9bcb --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -0,0 +1,45 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_ +#define RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_ + +#include "rclcpp/context.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace contexts +{ +namespace default_context +{ + +class DefaultContext : public rclcpp::Context +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext) + + RCLCPP_PUBLIC + DefaultContext(); +}; + +RCLCPP_PUBLIC +DefaultContext::SharedPtr +get_global_default_context(); + +} // namespace default_context +} // namespace contexts +} // namespace rclcpp + +#endif // RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/create_client.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/create_client.hpp new file mode 100644 index 0000000..94f6b21 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/create_client.hpp @@ -0,0 +1,56 @@ +// Copyright 2019 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 RCLCPP__CREATE_CLIENT_HPP_ +#define RCLCPP__CREATE_CLIENT_HPP_ + +#include +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +/// Create a service client with a given type. +/// \internal +template +typename rclcpp::Client::SharedPtr +create_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos_profile; + + auto cli = rclcpp::Client::make_shared( + node_base.get(), + node_graph, + service_name, + options); + + auto cli_base_ptr = std::dynamic_pointer_cast(cli); + node_services->add_client(cli_base_ptr, group); + return cli; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_CLIENT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/create_publisher.hpp new file mode 100644 index 0000000..811c18b --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/create_publisher.hpp @@ -0,0 +1,71 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CREATE_PUBLISHER_HPP_ +#define RCLCPP__CREATE_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rmw/qos_profiles.h" + +namespace rclcpp +{ + +/// Create and return a publisher of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface. + */ +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher, + typename NodeT> +std::shared_ptr +create_publisher( + NodeT & node, + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + // Extract the NodeTopicsInterface from the NodeT. + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics = get_node_topics_interface(node); + + // Create the publisher. + auto pub = node_topics->create_publisher( + topic_name, + rclcpp::create_publisher_factory(options), + qos + ); + + // Add the publisher to the node topics interface. + node_topics->add_publisher(pub, options.callback_group); + + return std::dynamic_pointer_cast(pub); +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_PUBLISHER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/create_service.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/create_service.hpp new file mode 100644 index 0000000..ac78462 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/create_service.hpp @@ -0,0 +1,58 @@ +// Copyright 2018 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 RCLCPP__CREATE_SERVICE_HPP_ +#define RCLCPP__CREATE_SERVICE_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +/// Create a service with a given type. +/// \internal +template +typename rclcpp::Service::SharedPtr +create_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + rclcpp::AnyServiceCallback any_service_callback; + any_service_callback.set(std::forward(callback)); + + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos_profile; + + auto serv = Service::make_shared( + node_base->get_shared_rcl_node_handle(), + service_name, any_service_callback, service_options); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services->add_service(serv_base_ptr, group); + return serv; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_SERVICE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/create_subscription.hpp new file mode 100644 index 0000000..9248254 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/create_subscription.hpp @@ -0,0 +1,81 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_ +#define RCLCPP__CREATE_SUBSCRIPTION_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/qos.hpp" +#include "rmw/qos_profiles.h" + +namespace rclcpp +{ + +/// Create and return a subscription of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + */ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + AllocatorT + >, + typename NodeT> +typename std::shared_ptr +create_subscription( + NodeT && node, + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) +) +{ + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics = get_node_topics_interface(std::forward(node)); + + auto factory = rclcpp::create_subscription_factory( + std::forward(callback), + options, + msg_mem_strat + ); + + auto sub = node_topics->create_subscription(topic_name, factory, qos); + node_topics->add_subscription(sub, options.callback_group); + + return std::dynamic_pointer_cast(sub); +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_SUBSCRIPTION_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/create_timer.hpp new file mode 100644 index 0000000..208474f --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/create_timer.hpp @@ -0,0 +1,73 @@ +// Copyright 2019 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 RCLCPP__CREATE_TIMER_HPP_ +#define RCLCPP__CREATE_TIMER_HPP_ + +#include +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" + +namespace rclcpp +{ +/// Create a timer with a given clock +/// \internal +template +typename rclcpp::TimerBase::SharedPtr +create_timer( + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers, + rclcpp::Clock::SharedPtr clock, + rclcpp::Duration period, + CallbackT && callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) +{ + auto timer = rclcpp::GenericTimer::make_shared( + clock, + period.to_chrono(), + std::forward(callback), + node_base->get_context()); + + node_timers->add_timer(timer, group); + return timer; +} + +/// Create a timer with a given clock +template +typename rclcpp::TimerBase::SharedPtr +create_timer( + NodeT node, + rclcpp::Clock::SharedPtr clock, + rclcpp::Duration period, + CallbackT && callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) +{ + return create_timer( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_timers_interface(node), + clock, + period, + std::forward(callback), + group); +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/detail/README.md b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/README.md new file mode 100644 index 0000000..a2712d6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/README.md @@ -0,0 +1,3 @@ +Notice that headers in this folder should only provide symbols in the rclcpp::detail namespace. + +Also that these headers are not considered part of the public API and are subject to change without notice. diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp new file mode 100644 index 0000000..e7196a1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp @@ -0,0 +1,54 @@ +// Copyright 2019 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 RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ +#define RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ + +#include + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" + +namespace rclcpp +{ + +namespace detail +{ + +/// Return the buffer type, resolving the "CallbackDefault" type to an actual type if needed. +template +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type, + const rclcpp::AnySubscriptionCallback & any_subscription_callback) +{ + rclcpp::IntraProcessBufferType resolved_buffer_type = buffer_type; + + // If the user has not specified a type for the intra-process buffer, use the callback's type. + if (resolved_buffer_type == IntraProcessBufferType::CallbackDefault) { + if (any_subscription_callback.use_take_shared_method()) { + resolved_buffer_type = IntraProcessBufferType::SharedPtr; + } else { + resolved_buffer_type = IntraProcessBufferType::UniquePtr; + } + } + + return resolved_buffer_type; +} + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp new file mode 100644 index 0000000..9098bfe --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp @@ -0,0 +1,56 @@ +// Copyright 2019 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 RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_ +#define RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_ + +#include + +#include "rclcpp/intra_process_setting.hpp" + +namespace rclcpp +{ + +namespace detail +{ + +/// Return whether or not intra process is enabled, resolving "NodeDefault" if needed. +template +bool +resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base) +{ + bool use_intra_process; + switch (options.use_intra_process_comm) { + case IntraProcessSetting::Enable: + use_intra_process = true; + break; + case IntraProcessSetting::Disable: + use_intra_process = false; + break; + case IntraProcessSetting::NodeDefault: + use_intra_process = node_base.get_use_intra_process_default(); + break; + default: + throw std::runtime_error("Unrecognized IntraProcessSetting value"); + break; + } + + return use_intra_process; +} + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp new file mode 100644 index 0000000..60bc930 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp @@ -0,0 +1,51 @@ +// Copyright 2019 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 RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_ +#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_ + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Mechanism for passing rmw implementation specific settings through the ROS interfaces. +class RCLCPP_PUBLIC RMWImplementationSpecificPayload +{ +public: + virtual + ~RMWImplementationSpecificPayload() = default; + + /// Return false if this class has not been customized, otherwise true. + /** + * It does this based on the value of the rmw implementation identifier that + * this class reports, and so it is important for a specialization of this + * class to override the get_rmw_implementation_identifier() method to return + * something other than nullptr. + */ + bool + has_been_customized() const; + + /// Derrived classes should override this and return the identifier of its rmw implementation. + virtual + const char * + get_implementation_identifier() const; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp new file mode 100644 index 0000000..08925a1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp @@ -0,0 +1,52 @@ +// Copyright 2019 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 RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_ +#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_ + +#include "rcl/publisher.h" + +#include "rclcpp/detail/rmw_implementation_specific_payload.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ + +class RCLCPP_PUBLIC RMWImplementationSpecificPublisherPayload + : public RMWImplementationSpecificPayload +{ +public: + ~RMWImplementationSpecificPublisherPayload() override = default; + + /// Opportunity for a derived class to inject information into the rcl options. + /** + * This is called after the rcl_publisher_options_t has been prepared by + * rclcpp, but before rcl_publisher_init() is called. + * + * The passed option is the rmw_publisher_options field of the + * rcl_publisher_options_t that will be passed to rcl_publisher_init(). + * + * By default the options are unmodified. + */ + virtual + void + modify_rmw_publisher_options(rmw_publisher_options_t & rmw_publisher_options) const; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp new file mode 100644 index 0000000..b909a35 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp @@ -0,0 +1,53 @@ +// Copyright 2019 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 RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_ +#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_ + +#include "rcl/subscription.h" + +#include "rclcpp/detail/rmw_implementation_specific_payload.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Subscription payload that may be rmw implementation specific. +class RCLCPP_PUBLIC RMWImplementationSpecificSubscriptionPayload + : public RMWImplementationSpecificPayload +{ +public: + ~RMWImplementationSpecificSubscriptionPayload() override = default; + + /// Opportunity for a derived class to inject information into the rcl options. + /** + * This is called after the rcl_subscription_options_t has been prepared by + * rclcpp, but before rcl_subscription_init() is called. + * + * The passed option is the rmw_subscription_options field of the + * rcl_subscription_options_t that will be passed to rcl_subscription_init(). + * + * By default the options are unmodified. + */ + virtual + void + modify_rmw_subscription_options(rmw_subscription_options_t & rmw_subscription_options) const; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/duration.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/duration.hpp new file mode 100644 index 0000000..5e09cc5 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/duration.hpp @@ -0,0 +1,119 @@ +// Copyright 2017 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 RCLCPP__DURATION_HPP_ +#define RCLCPP__DURATION_HPP_ + +#include + +#include "builtin_interfaces/msg/duration.hpp" +#include "rcl/time.h" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +class RCLCPP_PUBLIC Duration +{ +public: + Duration(int32_t seconds, uint32_t nanoseconds); + + // This constructor matches any numeric value - ints or floats + explicit Duration(rcl_duration_value_t nanoseconds); + + explicit Duration(std::chrono::nanoseconds nanoseconds); + + // This constructor matches any std::chrono value other than nanoseconds + // intentionally not using explicit to create a conversion constructor + template + // cppcheck-suppress noExplicitConstructor + Duration(const std::chrono::duration & duration) // NOLINT(runtime/explicit) + : Duration(std::chrono::duration_cast(duration)) + {} + + // cppcheck-suppress noExplicitConstructor + Duration(const builtin_interfaces::msg::Duration & duration_msg); // NOLINT(runtime/explicit) + + explicit Duration(const rcl_duration_t & duration); + + Duration(const Duration & rhs); + + virtual ~Duration() = default; + + operator builtin_interfaces::msg::Duration() const; + + // cppcheck-suppress operatorEq // this is a false positive from cppcheck + Duration & + operator=(const Duration & rhs); + + Duration & + operator=(const builtin_interfaces::msg::Duration & Duration_msg); + + bool + operator==(const rclcpp::Duration & rhs) const; + + bool + operator<(const rclcpp::Duration & rhs) const; + + bool + operator<=(const rclcpp::Duration & rhs) const; + + bool + operator>=(const rclcpp::Duration & rhs) const; + + bool + operator>(const rclcpp::Duration & rhs) const; + + Duration + operator+(const rclcpp::Duration & rhs) const; + + Duration + operator-(const rclcpp::Duration & rhs) const; + + static + Duration + max(); + + Duration + operator*(double scale) const; + + rcl_duration_value_t + nanoseconds() const; + + /// \return the duration in seconds as a floating point number. + /// \warning Depending on sizeof(double) there could be significant precision loss. + /// When an exact time is required use nanoseconds() instead. + double + seconds() const; + + // Create a duration object from a floating point number representing seconds + static Duration + from_seconds(double seconds); + + template + DurationT + to_chrono() const + { + return std::chrono::duration_cast(std::chrono::nanoseconds(this->nanoseconds())); + } + + rmw_time_t + to_rmw_time() const; + +private: + rcl_duration_t rcl_duration_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__DURATION_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/event.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/event.hpp new file mode 100644 index 0000000..988dba2 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/event.hpp @@ -0,0 +1,55 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EVENT_HPP_ +#define RCLCPP__EVENT_HPP_ + +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +class Event +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event) + + RCLCPP_PUBLIC + Event(); + + RCLCPP_PUBLIC + bool + set(); + + RCLCPP_PUBLIC + bool + check(); + + RCLCPP_PUBLIC + bool + check_and_clear(); + +private: + RCLCPP_DISABLE_COPY(Event) + + std::atomic_bool state_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__EVENT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/exceptions.hpp new file mode 100644 index 0000000..1976902 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/exceptions.hpp @@ -0,0 +1,260 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXCEPTIONS_HPP_ +#define RCLCPP__EXCEPTIONS_HPP_ + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/types.h" +#include "rclcpp/visibility_control.hpp" + +#include "rcpputils/join.hpp" + +namespace rclcpp +{ +namespace exceptions +{ + +/// Thrown when a method is trying to use a node, but it is invalid. +class InvalidNodeError : public std::runtime_error +{ +public: + InvalidNodeError() + : std::runtime_error("node is invalid") {} +}; + +/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid. +class NameValidationError : public std::invalid_argument +{ +public: + NameValidationError( + const char * name_type_, + const char * name_, + const char * error_msg_, + size_t invalid_index_) + : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)), + name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_) + {} + + static std::string + format_error( + const char * name_type, + const char * name, + const char * error_msg, + size_t invalid_index); + + const std::string name_type; + const std::string name; + const std::string error_msg; + const size_t invalid_index; +}; + +/// Thrown when a node name is invalid. +class InvalidNodeNameError : public NameValidationError +{ +public: + InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index) + : NameValidationError("node name", node_name, error_msg, invalid_index) + {} +}; + +/// Thrown when a node namespace is invalid. +class InvalidNamespaceError : public NameValidationError +{ +public: + InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("namespace", namespace_, error_msg, invalid_index) + {} +}; + +/// Thrown when a topic name is invalid. +class InvalidTopicNameError : public NameValidationError +{ +public: + InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("topic name", namespace_, error_msg, invalid_index) + {} +}; + +/// Thrown when a service name is invalid. +class InvalidServiceNameError : public NameValidationError +{ +public: + InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("service name", namespace_, error_msg, invalid_index) + {} +}; + +/// Throw a C++ std::exception which was created based on an rcl error. +/** + * Passing nullptr for reset_error is safe and will avoid calling any function + * to reset the error. + * + * \param ret the return code for the current error state + * \param prefix string to prefix to the error if applicable (not all errors have custom messages) + * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used + * \param reset_error function to be called before throwing which whill clear the error state + * \throws std::invalid_argument if ret is RCL_RET_OK + * \throws std::runtime_error if the rcl_get_error_state returns 0 + * \throws RCLErrorBase some child class exception based on ret + */ +/* *INDENT-OFF* */ // Uncrustify cannot yet understand [[noreturn]] properly +RCLCPP_PUBLIC +void +throw_from_rcl_error [[noreturn]] ( + rcl_ret_t ret, + const std::string & prefix = "", + const rcl_error_state_t * error_state = nullptr, + void (* reset_error)() = rcl_reset_error); +/* *INDENT-ON* */ + +class RCLErrorBase +{ +public: + RCLCPP_PUBLIC + RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state); + virtual ~RCLErrorBase() {} + + rcl_ret_t ret; + std::string message; + std::string file; + size_t line; + std::string formatted_message; +}; + +/// Created when the return code does not match one of the other specialized exceptions. +class RCLError : public RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); + RCLCPP_PUBLIC + RCLError(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Created when the ret is RCL_RET_BAD_ALLOC. +class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc +{ +public: + RCLCPP_PUBLIC + RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state); + RCLCPP_PUBLIC + explicit RCLBadAlloc(const RCLErrorBase & base_exc); +}; + +/// Created when the ret is RCL_RET_INVALID_ARGUMENT. +class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument +{ +public: + RCLCPP_PUBLIC + RCLInvalidArgument( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix); + RCLCPP_PUBLIC + RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Created when the ret is RCL_RET_INVALID_ROS_ARGS. +class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + RCLInvalidROSArgsError( + rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); + RCLCPP_PUBLIC + RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Thrown when unparsed ROS specific arguments are found. +class UnknownROSArgsError : public std::runtime_error +{ +public: + explicit UnknownROSArgsError(std::vector && unknown_ros_args_in) + : std::runtime_error( + "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"), + unknown_ros_args(unknown_ros_args_in) + { + } + + const std::vector unknown_ros_args; +}; + +/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. +class InvalidEventError : public std::runtime_error +{ +public: + InvalidEventError() + : std::runtime_error("event is invalid") {} +}; + +/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected. +class EventNotRegisteredError : public std::runtime_error +{ +public: + EventNotRegisteredError() + : std::runtime_error("event already registered") {} +}; + +/// Thrown if passed parameters are inconsistent or invalid +class InvalidParametersException : public std::runtime_error +{ +public: + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if passed parameter value is invalid. +class InvalidParameterValueException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is already declared. +class ParameterAlreadyDeclaredException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring. +class ParameterNotDeclaredException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is immutable and therefore cannot be undeclared. +class ParameterImmutableException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is modified while in a set callback. +class ParameterModifiedInCallbackException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +} // namespace exceptions +} // namespace rclcpp + +#endif // RCLCPP__EXCEPTIONS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/executor.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/executor.hpp new file mode 100644 index 0000000..dd24bc3 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/executor.hpp @@ -0,0 +1,380 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTOR_HPP_ +#define RCLCPP__EXECUTOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/guard_condition.h" +#include "rcl/wait.h" + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/memory_strategies.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/scope_exit.hpp" + +namespace rclcpp +{ + +// Forward declaration is used in convenience method signature. +class Node; + +namespace executor +{ + +/// Return codes to be used with spin_until_future_complete. +/** + * SUCCESS: The future is complete and can be accessed with "get" without blocking. + * This does not indicate that the operation succeeded; "get" may still throw an exception. + * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. + * TIMEOUT: Spinning timed out. + */ +enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; + +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const FutureReturnCode & future_return_code); + +RCLCPP_PUBLIC +std::string +to_string(const FutureReturnCode & future_return_code); + +/// +/** + * Options to be passed to the executor constructor. + */ +struct ExecutorArgs +{ + ExecutorArgs() + : memory_strategy(memory_strategies::create_default_strategy()), + context(rclcpp::contexts::default_context::get_global_default_context()), + max_conditions(0) + {} + + memory_strategy::MemoryStrategy::SharedPtr memory_strategy; + std::shared_ptr context; + size_t max_conditions; +}; + +static inline ExecutorArgs create_default_executor_arguments() +{ + return ExecutorArgs(); +} + +/// Coordinate the order and timing of available communication tasks. +/** + * Executor provides spin functions (including spin_node_once and spin_some). + * It coordinates the nodes and callback groups by looking for available work and completing it, + * based on the threading or concurrency scheme provided by the subclass implementation. + * An example of available work is executing a subscription callback, or a timer callback. + * The executor structure allows for a decoupling of the communication graph and the execution + * model. + * See SingleThreadedExecutor and MultiThreadedExecutor for examples of execution paradigms. + */ +class Executor +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor) + + /// Default constructor. + // \param[in] ms The memory strategy to be used with this executor. + RCLCPP_PUBLIC + explicit Executor(const ExecutorArgs & args = ExecutorArgs()); + + /// Default destructor. + RCLCPP_PUBLIC + virtual ~Executor(); + + /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely. + // It is up to the implementation of Executor to implement spin. + virtual void + spin() = 0; + + /// Add a node to the executor. + /** + * An executor can have zero or more nodes which provide work during `spin` functions. + * \param[in] node_ptr Shared pointer to the node to be added. + * \param[in] notify True to trigger the interrupt guard condition during this function. If + * the executor is blocked at the rmw layer while waiting for work and it is notified that a new + * node was added, it will wake up. + */ + RCLCPP_PUBLIC + virtual void + add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + virtual void + add_node(std::shared_ptr node_ptr, bool notify = true); + + /// Remove a node from the executor. + /** + * \param[in] node_ptr Shared pointer to the node to remove. + * \param[in] notify True to trigger the interrupt guard condition and wake up the executor. + * This is useful if the last node was removed from the executor while the executor was blocked + * waiting for work in another thread, because otherwise the executor would never be notified. + */ + RCLCPP_PUBLIC + virtual void + remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + virtual void + remove_node(std::shared_ptr node_ptr, bool notify = true); + + /// Add a node to executor, execute the next available unit of work, and remove the node. + /** + * \param[in] node Shared pointer to the node to add. + * \param[in] timeout How long to wait for work to become available. Negative values cause + * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this + * function to be non-blocking. + */ + template + void + spin_node_once( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return spin_node_once_nanoseconds( + node, + std::chrono::duration_cast(timeout) + ); + } + + /// Convenience function which takes Node and forwards NodeBaseInterface. + template + void + spin_node_once( + std::shared_ptr node, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return spin_node_once_nanoseconds( + node->get_node_base_interface(), + std::chrono::duration_cast(timeout) + ); + } + + /// Add a node, complete all immediately available work, and remove the node. + /** + * \param[in] node Shared pointer to the node to add. + */ + RCLCPP_PUBLIC + void + spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + void + spin_node_some(std::shared_ptr node); + + /// Complete all available queued work without blocking. + /** + * This function can be overridden. The default implementation is suitable for a + * single-threaded model of execution. + * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function + * to block (which may have unintended consequences). + * + * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit. + * Note that spin_some() may take longer than this time as it only returns once max_duration has + * been exceeded. + */ + RCLCPP_PUBLIC + virtual void + spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)); + + RCLCPP_PUBLIC + virtual void + spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. + /** + * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be + * accessed without blocking (though it may still throw an exception). + * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. + * `-1` is block forever, `0` is non-blocking. + * If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return + * code. + * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. + */ + template + FutureReturnCode + spin_until_future_complete( + std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + + // Check the future before entering the while loop. + // If the future is already complete, don't try to spin. + std::future_status status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_future_complete() called while already spinning"); + } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { + // Do one item of work. + spin_once_impl(timeout_left); + + // Check if the future is set, return SUCCESS if it is. + status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + + // The future did not complete before ok() returned false, return INTERRUPTED. + return FutureReturnCode::INTERRUPTED; + } + + /// Cancel any running spin* function, causing it to return. + /* This function can be called asynchonously from any thread. */ + RCLCPP_PUBLIC + void + cancel(); + + /// Support dynamic switching of the memory strategy. + /** + * Switching the memory strategy while the executor is spinning in another threading could have + * unintended consequences. + * \param[in] memory_strategy Shared pointer to the memory strategy to set. + */ + RCLCPP_PUBLIC + void + set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); + +protected: + RCLCPP_PUBLIC + void + spin_node_once_nanoseconds( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds timeout); + + /// Find the next available executable and do the work associated with it. + /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription, + * service, client). + */ + RCLCPP_PUBLIC + void + execute_any_executable(AnyExecutable & any_exec); + + RCLCPP_PUBLIC + static void + execute_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription); + + RCLCPP_PUBLIC + static void + execute_timer(rclcpp::TimerBase::SharedPtr timer); + + RCLCPP_PUBLIC + static void + execute_service(rclcpp::ServiceBase::SharedPtr service); + + RCLCPP_PUBLIC + static void + execute_client(rclcpp::ClientBase::SharedPtr client); + + RCLCPP_PUBLIC + void + wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group); + + RCLCPP_PUBLIC + rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); + + RCLCPP_PUBLIC + bool + get_next_ready_executable(AnyExecutable & any_executable); + + RCLCPP_PUBLIC + bool + get_next_executable( + AnyExecutable & any_executable, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + + /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. + std::atomic_bool spinning; + + /// Guard condition for signaling the rmw layer to wake up for special events. + rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + + /// Wait set for managing entities that the rmw layer waits on. + rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); + + // Mutex to protect the subsequent memory_strategy_. + std::mutex memory_strategy_mutex_; + + /// The memory strategy: an interface for handling user-defined memory allocation strategies. + memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; + + /// The context associated with this executor. + std::shared_ptr context_; + +private: + RCLCPP_DISABLE_COPY(Executor) + + RCLCPP_PUBLIC + void + spin_once_impl(std::chrono::nanoseconds timeout); + + std::list weak_nodes_; + std::list guard_conditions_; +}; + +} // namespace executor +} // namespace rclcpp + +#endif // RCLCPP__EXECUTOR_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/executors.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/executors.hpp new file mode 100644 index 0000000..7417098 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/executors.hpp @@ -0,0 +1,126 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS_HPP_ +#define RCLCPP__EXECUTORS_HPP_ + +#include +#include + +#include "rclcpp/executors/multi_threaded_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Create a default single-threaded executor and execute any immediately available work. +/** \param[in] node_ptr Shared pointer to the node to spin. */ +RCLCPP_PUBLIC +void +spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + +RCLCPP_PUBLIC +void +spin_some(rclcpp::Node::SharedPtr node_ptr); + +/// Create a default single-threaded executor and spin the specified node. +/** \param[in] node_ptr Shared pointer to the node to spin. */ +RCLCPP_PUBLIC +void +spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + +RCLCPP_PUBLIC +void +spin(rclcpp::Node::SharedPtr node_ptr); + +namespace executors +{ + +using rclcpp::executors::MultiThreadedExecutor; +using rclcpp::executors::SingleThreadedExecutor; + +/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. +/** + * \param[in] executor The executor which will spin the node. + * \param[in] node_ptr The node to spin. + * \param[in] future The future to wait on. If `SUCCESS`, the future is safe to + * access after this function + * \param[in] timeout Optional timeout parameter, which gets passed to + * Executor::spin_node_once. + * `-1` is block forever, `0` is non-blocking. + * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. + * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. + */ +template +rclcpp::executor::FutureReturnCode +spin_node_until_future_complete( + rclcpp::executor::Executor & executor, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + executor.add_node(node_ptr); + auto retcode = executor.spin_until_future_complete(future, timeout); + executor.remove_node(node_ptr); + return retcode; +} + +template +rclcpp::executor::FutureReturnCode +spin_node_until_future_complete( + rclcpp::executor::Executor & executor, + std::shared_ptr node_ptr, + std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::executors::spin_node_until_future_complete( + executor, + node_ptr->get_node_base_interface(), + future, + timeout); +} + +} // namespace executors + +template +rclcpp::executor::FutureReturnCode +spin_until_future_complete( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + rclcpp::executors::SingleThreadedExecutor executor; + return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); +} + +template +rclcpp::executor::FutureReturnCode +spin_until_future_complete( + std::shared_ptr node_ptr, + std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); +} + +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp new file mode 100644 index 0000000..316c016 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -0,0 +1,90 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ +#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/executor.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/memory_strategies.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace executors +{ + +class MultiThreadedExecutor : public executor::Executor +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor) + + /// Constructor for MultiThreadedExecutor. + /** + * For the yield_before_execute option, when true std::this_thread::yield() + * will be called after acquiring work (as an AnyExecutable) and + * releasing the spinning lock, but before executing the work. + * This is useful for reproducing some bugs related to taking work more than + * once. + * + * \param args common arguments for all executors + * \param number_of_threads number of threads to have in the thread pool, + * the default 0 will use the number of cpu cores found instead + * \param yield_before_execute if true std::this_thread::yield() is called + */ + RCLCPP_PUBLIC + MultiThreadedExecutor( + const executor::ExecutorArgs & args = executor::ExecutorArgs(), + size_t number_of_threads = 0, + bool yield_before_execute = false, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + + RCLCPP_PUBLIC + virtual ~MultiThreadedExecutor(); + + RCLCPP_PUBLIC + void + spin(); + + RCLCPP_PUBLIC + size_t + get_number_of_threads(); + +protected: + RCLCPP_PUBLIC + void + run(size_t this_thread_number); + +private: + RCLCPP_DISABLE_COPY(MultiThreadedExecutor) + + std::mutex wait_mutex_; + size_t number_of_threads_; + bool yield_before_execute_; + std::chrono::nanoseconds next_exec_timeout_; + + std::set scheduled_timers_; +}; + +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp new file mode 100644 index 0000000..f6bcfa7 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -0,0 +1,68 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_ +#define RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_ + +#include + +#include +#include +#include +#include + +#include "rclcpp/executor.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/memory_strategies.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/rate.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace executors +{ + +/// Single-threaded executor implementation +// This is the default executor created by rclcpp::spin. +class SingleThreadedExecutor : public executor::Executor +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor) + + /// Default constructor. See the default constructor for Executor. + RCLCPP_PUBLIC + SingleThreadedExecutor( + const executor::ExecutorArgs & args = executor::ExecutorArgs()); + + /// Default destrcutor. + RCLCPP_PUBLIC + virtual ~SingleThreadedExecutor(); + + /// Single-threaded implementation of spin. + // This function will block until work comes in, execute it, and keep blocking. + // It will only be interrupt by a CTRL-C (managed by the global signal handler). + RCLCPP_PUBLIC + void + spin(); + +private: + RCLCPP_DISABLE_COPY(SingleThreadedExecutor) +}; + +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp new file mode 100644 index 0000000..5454dfa --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp @@ -0,0 +1,63 @@ +// Copyright 2017 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 RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_ +#define RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_ + +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Expand a topic or service name and throw if it is not valid. +/** + * This function can be used to "just" validate a topic or service name too, + * since expanding the topic name is required to fully validate a name. + * + * If the name is invalid, then InvalidTopicNameError is thrown or + * InvalidServiceNameError if is_service is true. + * + * This function can take any form of a topic or service name, i.e. it does not + * have to be a fully qualified name. + * The node name and namespace are used to expand it if necessary while + * validating it. + * + * Expansion is done with rcl_expand_topic_name. + * The validation is doen with rcl_validate_topic_name and + * rmw_validate_full_topic_name, so details about failures can be found in the + * documentation for those functions. + * + * \param name the topic or service name to be validated + * \param node_name the name of the node associated with the name + * \param namespace_ the namespace of the node associated with the name + * \param is_service if true InvalidServiceNameError is thrown instead + * \returns expanded (and validated) topic name + * \throws InvalidTopicNameError if name is invalid and is_service is false + * \throws InvalidServiceNameError if name is invalid and is_service is true + * \throws std::bad_alloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs + */ +RCLCPP_PUBLIC +std::string +expand_topic_or_service_name( + const std::string & name, + const std::string & node_name, + const std::string & namespace_, + bool is_service = false); + +} // namespace rclcpp + +#endif // RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/README.md b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/README.md new file mode 100644 index 0000000..38ca07c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/README.md @@ -0,0 +1,4 @@ +Notice that headers in this folder should only provide symbols in the rclcpp::experimental namespace. + +Also notice that these headers are not considered part of the public API as they have not yet been stabilized. +And therefore they are subject to change without notice. diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp new file mode 100644 index 0000000..1e53461 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -0,0 +1,42 @@ +// Copyright 2019 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 RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +template +class BufferImplementationBase +{ +public: + virtual ~BufferImplementationBase() {} + + virtual BufferT dequeue() = 0; + virtual void enqueue(BufferT request) = 0; + + virtual void clear() = 0; + virtual bool has_data() const = 0; +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp new file mode 100644 index 0000000..aef38a0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -0,0 +1,241 @@ +// Copyright 2019 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 RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/macros.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +class IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + + virtual ~IntraProcessBufferBase() {} + + virtual void clear() = 0; + + virtual bool has_data() const = 0; + virtual bool use_take_shared_method() const = 0; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete> +class IntraProcessBuffer : public IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBuffer) + + virtual ~IntraProcessBuffer() {} + + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + virtual void add_shared(MessageSharedPtr msg) = 0; + virtual void add_unique(MessageUniquePtr msg) = 0; + + virtual MessageSharedPtr consume_shared() = 0; + virtual MessageUniquePtr consume_unique() = 0; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete, + typename BufferT = std::unique_ptr> +class TypedIntraProcessBuffer : public IntraProcessBuffer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(TypedIntraProcessBuffer) + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + explicit + TypedIntraProcessBuffer( + std::unique_ptr> buffer_impl, + std::shared_ptr allocator = nullptr) + { + bool valid_type = (std::is_same::value || + std::is_same::value); + if (!valid_type) { + throw std::runtime_error("Creating TypedIntraProcessBuffer with not valid BufferT"); + } + + buffer_ = std::move(buffer_impl); + + if (!allocator) { + message_allocator_ = std::make_shared(); + } else { + message_allocator_ = std::make_shared(*allocator.get()); + } + } + + virtual ~TypedIntraProcessBuffer() {} + + void add_shared(MessageSharedPtr msg) override + { + add_shared_impl(std::move(msg)); + } + + void add_unique(MessageUniquePtr msg) override + { + buffer_->enqueue(std::move(msg)); + } + + MessageSharedPtr consume_shared() override + { + return consume_shared_impl(); + } + + MessageUniquePtr consume_unique() override + { + return consume_unique_impl(); + } + + bool has_data() const override + { + return buffer_->has_data(); + } + + void clear() override + { + buffer_->clear(); + } + + bool use_take_shared_method() const override + { + return std::is_same::value; + } + +private: + std::unique_ptr> buffer_; + + std::shared_ptr message_allocator_; + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value + >::type + add_shared_impl(MessageSharedPtr shared_msg) + { + buffer_->enqueue(std::move(shared_msg)); + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + std::is_same::value + >::type + add_shared_impl(MessageSharedPtr shared_msg) + { + // This should not happen: here a copy is unconditionally made, while the intra-process manager + // can decide whether a copy is needed depending on the number and the type of buffers + + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(shared_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + + buffer_->enqueue(std::move(unique_msg)); + } + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + MessageSharedPtr + >::type + consume_shared_impl() + { + return buffer_->dequeue(); + } + + // MessageUniquePtr to MessageSharedPtr + template + typename std::enable_if< + (std::is_same::value), + MessageSharedPtr + >::type + consume_shared_impl() + { + // automatic cast from unique ptr to shared ptr + return buffer_->dequeue(); + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + (std::is_same::value), + MessageUniquePtr + >::type + consume_unique_impl() + { + MessageSharedPtr buffer_msg = buffer_->dequeue(); + + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(buffer_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *buffer_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + + return unique_msg; + } + + // MessageUniquePtr to MessageUniquePtr + template + typename std::enable_if< + (std::is_same::value), + MessageUniquePtr + >::type + consume_unique_impl() + { + return buffer_->dequeue(); + } +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp new file mode 100644 index 0000000..ea37c23 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -0,0 +1,122 @@ +// Copyright 2019 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 RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +template +class RingBufferImplementation : public BufferImplementationBase +{ +public: + explicit RingBufferImplementation(size_t capacity) + : capacity_(capacity), + ring_buffer_(capacity), + write_index_(capacity_ - 1), + read_index_(0), + size_(0) + { + if (capacity == 0) { + throw std::invalid_argument("capacity must be a positive, non-zero value"); + } + } + + virtual ~RingBufferImplementation() {} + + void enqueue(BufferT request) + { + std::lock_guard lock(mutex_); + + write_index_ = next(write_index_); + ring_buffer_[write_index_] = std::move(request); + + if (is_full()) { + read_index_ = next(read_index_); + } else { + size_++; + } + } + + BufferT dequeue() + { + std::lock_guard lock(mutex_); + + if (!has_data()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer"); + throw std::runtime_error("Calling dequeue on empty intra-process buffer"); + } + + auto request = std::move(ring_buffer_[read_index_]); + read_index_ = next(read_index_); + + size_--; + + return request; + } + + inline size_t next(size_t val) + { + return (val + 1) % capacity_; + } + + inline bool has_data() const + { + return size_ != 0; + } + + inline bool is_full() + { + return size_ == capacity_; + } + + void clear() {} + +private: + size_t capacity_; + + std::vector ring_buffer_; + + size_t write_index_; + size_t read_index_; + size_t size_; + + std::mutex mutex_; +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp new file mode 100644 index 0000000..ff57a84 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp @@ -0,0 +1,99 @@ +// Copyright 2019 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 RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include + +#include "rcl/subscription.h" + +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> +typename rclcpp::experimental::buffers::IntraProcessBuffer::UniquePtr +create_intra_process_buffer( + IntraProcessBufferType buffer_type, + rmw_qos_profile_t qos, + std::shared_ptr allocator) +{ + using MessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + size_t buffer_size = qos.depth; + + using rclcpp::experimental::buffers::IntraProcessBuffer; + typename IntraProcessBuffer::UniquePtr buffer; + + switch (buffer_type) { + case IntraProcessBufferType::SharedPtr: + { + using BufferT = MessageSharedPtr; + + auto buffer_implementation = + std::make_unique>( + buffer_size); + + // Construct the intra_process_buffer + buffer = + std::make_unique>( + std::move(buffer_implementation), + allocator); + + break; + } + case IntraProcessBufferType::UniquePtr: + { + using BufferT = MessageUniquePtr; + + auto buffer_implementation = + std::make_unique>( + buffer_size); + + // Construct the intra_process_buffer + buffer = + std::make_unique>( + std::move(buffer_implementation), + allocator); + + break; + } + default: + { + throw std::runtime_error("Unrecognized IntraProcessBufferType value"); + break; + } + } + + return buffer; +} + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp new file mode 100644 index 0000000..7c216c6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -0,0 +1,426 @@ +// Copyright 2019 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 RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ +#define RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/subscription_intra_process.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +namespace experimental +{ + +/// This class performs intra process communication between nodes. +/** + * This class is used in the creation of publishers and subscriptions. + * A singleton instance of this class is owned by a rclcpp::Context and a + * rclcpp::Node can use an associated Context to get an instance of this class. + * Nodes which do not have a common Context will not exchange intra process + * messages because they do not share access to the same instance of this class. + * + * When a Node creates a subscription, it can also create a helper class, + * called SubscriptionIntraProcess, meant to receive intra process messages. + * It can be registered with this class. + * It is also allocated an id which is unique among all publishers + * and subscriptions in this process and that is associated to the subscription. + * + * When a Node creates a publisher, as with subscriptions, a helper class can + * be registered with this class. + * This is required in order to publish intra-process messages. + * It is also allocated an id which is unique among all publishers + * and subscriptions in this process and that is associated to the publisher. + * + * When a publisher or a subscription are registered, this class checks to see + * which other subscriptions or publishers it will communicate with, + * i.e. they have the same topic and compatible QoS. + * + * When the user publishes a message, if intra-process communication is enabled + * on the publisher, the message is given to this class. + * Using the publisher id, a list of recipients for the message is selected. + * For each subscription in the list, this class stores the message, whether + * sharing ownership or making a copy, in a buffer associated with the + * subscription helper class. + * + * The subscription helper class contains a buffer where published + * intra-process messages are stored until they are taken from the subscription. + * Depending on the data type stored in the buffer, the subscription helper + * class can request either shared or exclusive ownership on the message. + * + * Thus, when an intra-process message is published, this class knows how many + * intra-process subscriptions needs it and how many require ownership. + * This information allows this class to operate efficiently by performing the + * fewest number of copies of the message required. + * + * This class is neither CopyConstructable nor CopyAssignable. + */ +class IntraProcessManager +{ +private: + RCLCPP_DISABLE_COPY(IntraProcessManager) + +public: + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager) + + RCLCPP_PUBLIC + IntraProcessManager(); + + RCLCPP_PUBLIC + virtual ~IntraProcessManager(); + + /// Register a subscription with the manager, returns subscriptions unique id. + /** + * This method stores the subscription intra process object, together with + * the information of its wrapped subscription (i.e. topic name and QoS). + * + * In addition this generates a unique intra process id for the subscription. + * + * \param subscription the SubscriptionIntraProcess to register. + * \return an unsigned 64-bit integer which is the subscription's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription); + + /// Unregister a subscription using the subscription's unique id. + /** + * This method does not allocate memory. + * + * \param intra_process_subscription_id id of the subscription to remove. + */ + RCLCPP_PUBLIC + void + remove_subscription(uint64_t intra_process_subscription_id); + + /// Register a publisher with the manager, returns the publisher unique id. + /** + * This method stores the publisher intra process object, together with + * the information of its wrapped publisher (i.e. topic name and QoS). + * + * In addition this generates a unique intra process id for the publisher. + * + * \param publisher publisher to be registered with the manager. + * \return an unsigned 64-bit integer which is the publisher's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_publisher(rclcpp::PublisherBase::SharedPtr publisher); + + /// Unregister a publisher using the publisher's unique id. + /** + * This method does not allocate memory. + * + * \param intra_process_publisher_id id of the publisher to remove. + */ + RCLCPP_PUBLIC + void + remove_publisher(uint64_t intra_process_publisher_id); + + /// Publishes an intra-process message, passed as a unique pointer. + /** + * This is one of the two methods for publishing intra-process. + * + * Using the intra-process publisher id, a list of recipients is obtained. + * This list is split in half, depending whether they require ownership or not. + * + * This particular method takes a unique pointer as input. + * The pointer can be promoted to a shared pointer and passed to all the subscriptions + * that do not require ownership. + * In case of subscriptions requiring ownership, the message will be copied for all of + * them except the last one, when ownership can be transferred. + * + * This method can save an additional copy compared to the shared pointer one. + * + * This method can throw an exception if the publisher id is not found or + * if the publisher shared_ptr given to add_publisher has gone out of scope. + * + * This method does allocate memory. + * + * \param intra_process_publisher_id the id of the publisher of this message. + * \param message the message that is being stored. + */ + template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> + void + do_intra_process_publish( + uint64_t intra_process_publisher_id, + std::unique_ptr message, + std::shared_ptr::allocator_type> allocator) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + + std::shared_lock lock(mutex_); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling do_intra_process_publish for invalid or no longer existing publisher id"); + return; + } + const auto & sub_ids = publisher_it->second; + + if (sub_ids.take_ownership_subscriptions.empty()) { + // None of the buffers require ownership, so we promote the pointer + std::shared_ptr msg = std::move(message); + + this->template add_shared_msg_to_buffers(msg, sub_ids.take_shared_subscriptions); + } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT + sub_ids.take_shared_subscriptions.size() <= 1) + { + // There is at maximum 1 buffer that does not require ownership. + // So we this case is equivalent to all the buffers requiring ownership + + // Merge the two vector of ids into a unique one + std::vector concatenated_vector(sub_ids.take_shared_subscriptions); + concatenated_vector.insert( + concatenated_vector.end(), + sub_ids.take_ownership_subscriptions.begin(), + sub_ids.take_ownership_subscriptions.end()); + + this->template add_owned_msg_to_buffers( + std::move(message), + concatenated_vector, + allocator); + } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT + sub_ids.take_shared_subscriptions.size() > 1) + { + // Construct a new shared pointer from the message + // for the buffers that do not require ownership + auto shared_msg = std::allocate_shared(*allocator, *message); + + this->template add_shared_msg_to_buffers(shared_msg, + sub_ids.take_shared_subscriptions); + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); + } + } + + template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> + std::shared_ptr + do_intra_process_publish_and_return_shared( + uint64_t intra_process_publisher_id, + std::unique_ptr message, + std::shared_ptr::allocator_type> allocator) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + + std::shared_lock lock(mutex_); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling do_intra_process_publish for invalid or no longer existing publisher id"); + return nullptr; + } + const auto & sub_ids = publisher_it->second; + + if (sub_ids.take_ownership_subscriptions.empty()) { + // If there are no owning, just convert to shared. + std::shared_ptr shared_msg = std::move(message); + if (!sub_ids.take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers(shared_msg, + sub_ids.take_shared_subscriptions); + } + return shared_msg; + } else { + // Construct a new shared pointer from the message for the buffers that + // do not require ownership and to return. + auto shared_msg = std::allocate_shared(*allocator, *message); + + if (!sub_ids.take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers( + shared_msg, + sub_ids.take_shared_subscriptions); + } + if (!sub_ids.take_ownership_subscriptions.empty()) { + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); + } + + return shared_msg; + } + } + + /// Return true if the given rmw_gid_t matches any stored Publishers. + RCLCPP_PUBLIC + bool + matches_any_publishers(const rmw_gid_t * id) const; + + /// Return the number of intraprocess subscriptions that are matched with a given publisher id. + RCLCPP_PUBLIC + size_t + get_subscription_count(uint64_t intra_process_publisher_id) const; + + RCLCPP_PUBLIC + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr + get_subscription_intra_process(uint64_t intra_process_subscription_id); + +private: + struct SubscriptionInfo + { + SubscriptionInfo() = default; + + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription; + rmw_qos_profile_t qos; + const char * topic_name; + bool use_take_shared_method; + }; + + struct PublisherInfo + { + PublisherInfo() = default; + + rclcpp::PublisherBase::WeakPtr publisher; + rmw_qos_profile_t qos; + const char * topic_name; + }; + + struct SplittedSubscriptions + { + std::vector take_shared_subscriptions; + std::vector take_ownership_subscriptions; + }; + + using SubscriptionMap = + std::unordered_map; + + using PublisherMap = + std::unordered_map; + + using PublisherToSubscriptionIdsMap = + std::unordered_map; + + RCLCPP_PUBLIC + static + uint64_t + get_next_unique_id(); + + RCLCPP_PUBLIC + void + insert_sub_id_for_pub(uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method); + + RCLCPP_PUBLIC + bool + can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const; + + template + void + add_shared_msg_to_buffers( + std::shared_ptr message, + std::vector subscription_ids) + { + for (auto id : subscription_ids) { + auto subscription_it = subscriptions_.find(id); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.subscription; + + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); + + subscription->provide_intra_process_message(message); + } + } + + template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> + void + add_owned_msg_to_buffers( + std::unique_ptr message, + std::vector subscription_ids, + std::shared_ptr::allocator_type> allocator) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageUniquePtr = std::unique_ptr; + + for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { + auto subscription_it = subscriptions_.find(*it); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.subscription; + + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); + + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + subscription->provide_intra_process_message(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + MessageUniquePtr copy_message; + Deleter deleter = message.get_deleter(); + auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); + MessageAllocTraits::construct(*allocator.get(), ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + subscription->provide_intra_process_message(std::move(copy_message)); + } + } + } + + PublisherToSubscriptionIdsMap pub_to_subs_; + SubscriptionMap subscriptions_; + PublisherMap publishers_; + + mutable std::shared_timed_mutex mutex_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp new file mode 100644 index 0000000..8a24c0c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -0,0 +1,173 @@ +// Copyright 2019 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 RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ + +#include + +#include +#include +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete, + typename CallbackMessageT = MessageT> +class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + MessageT, + Alloc, + Deleter + >::UniquePtr; + + SubscriptionIntraProcess( + AnySubscriptionCallback callback, + std::shared_ptr allocator, + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + rmw_qos_profile_t qos_profile, + rclcpp::IntraProcessBufferType buffer_type) + : SubscriptionIntraProcessBase(topic_name, qos_profile), + any_callback_(callback) + { + if (!std::is_same::value) { + throw std::runtime_error("SubscriptionIntraProcess wrong callback type"); + } + + // Create the intra-process buffer. + buffer_ = rclcpp::experimental::create_intra_process_buffer( + buffer_type, + qos_profile, + allocator); + + // Create the guard condition. + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options(); + + gc_ = rcl_get_zero_initialized_guard_condition(); + rcl_ret_t ret = rcl_guard_condition_init( + &gc_, context->get_rcl_context().get(), guard_condition_options); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition"); + } + + TRACEPOINT( + rclcpp_subscription_callback_added, + (const void *)this, + (const void *)&any_callback_); + // The callback object gets copied, so if registration is done too early/before this point + // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later + // in subsequent tracepoints. + any_callback_.register_callback_for_tracing(); + } + + bool + is_ready(rcl_wait_set_t * wait_set) + { + (void)wait_set; + return buffer_->has_data(); + } + + void execute() + { + execute_impl(); + } + + void + provide_intra_process_message(ConstMessageSharedPtr message) + { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + } + + void + provide_intra_process_message(MessageUniquePtr message) + { + buffer_->add_unique(std::move(message)); + trigger_guard_condition(); + } + + bool + use_take_shared_method() const + { + return buffer_->use_take_shared_method(); + } + +private: + void + trigger_guard_condition() + { + rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); + (void)ret; + } + + template + typename std::enable_if::value, void>::type + execute_impl() + { + throw std::runtime_error("Subscription intra-process can't handle serialized messages"); + } + + template + typename std::enable_if::value, void>::type + execute_impl() + { + rmw_message_info_t msg_info; + msg_info.from_intra_process = true; + + if (any_callback_.use_take_shared_method()) { + ConstMessageSharedPtr msg = buffer_->consume_shared(); + any_callback_.dispatch_intra_process(msg, msg_info); + } else { + MessageUniquePtr msg = buffer_->consume_unique(); + any_callback_.dispatch_intra_process(std::move(msg), msg_info); + } + } + + AnySubscriptionCallback any_callback_; + BufferUniquePtr buffer_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp new file mode 100644 index 0000000..7afd68a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -0,0 +1,88 @@ +// Copyright 2019 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 RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +class SubscriptionIntraProcessBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + + RCLCPP_PUBLIC + SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile) + : topic_name_(topic_name), qos_profile_(qos_profile) + {} + + virtual ~SubscriptionIntraProcessBase() = default; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() {return 1;} + + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set); + + virtual bool + is_ready(rcl_wait_set_t * wait_set) = 0; + + virtual void + execute() = 0; + + virtual bool + use_take_shared_method() const = 0; + + RCLCPP_PUBLIC + const char * + get_topic_name() const; + + RCLCPP_PUBLIC + rmw_qos_profile_t + get_actual_qos() const; + +protected: + std::recursive_mutex reentrant_mutex_; + rcl_guard_condition_t gc_; + +private: + virtual void + trigger_guard_condition() = 0; + + std::string topic_name_; + rmw_qos_profile_t qos_profile_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/function_traits.hpp new file mode 100644 index 0000000..f5c56b0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/function_traits.hpp @@ -0,0 +1,169 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__FUNCTION_TRAITS_HPP_ +#define RCLCPP__FUNCTION_TRAITS_HPP_ + +#include +#include +#include + +namespace rclcpp +{ + +namespace function_traits +{ + +/* NOTE(esteve): + * We support service callbacks that can optionally take the request id, + * which should be possible with two overloaded create_service methods, + * but unfortunately std::function's constructor on VS2015 is too greedy, + * so we need a mechanism for checking the arity and the type of each argument + * in a callback function. + * See http://blogs.msdn.com/b/vcblog/archive/2015/06/19/c-11-14-17-features-in-vs-2015-rtm.aspx + */ + +// Remove the first item in a tuple +template +struct tuple_tail; + +template +struct tuple_tail> +{ + using type = std::tuple; +}; + +// std::function +template +struct function_traits +{ + using arguments = typename tuple_tail< + typename function_traits::arguments>::type; + + static constexpr std::size_t arity = std::tuple_size::value; + + template + using argument_type = typename std::tuple_element::type; + + using return_type = typename function_traits::return_type; +}; + +// Free functions +template +struct function_traits +{ + using arguments = std::tuple; + + static constexpr std::size_t arity = std::tuple_size::value; + + template + using argument_type = typename std::tuple_element::type; + + using return_type = ReturnTypeT; +}; + +// Function pointers +template +struct function_traits: function_traits +{}; + +// std::bind for object methods +template +#if defined _LIBCPP_VERSION // libc++ (Clang) +struct function_traits> +#elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) +struct function_traits> +#elif defined __GLIBCXX__ // glibc++ (GNU C++) +struct function_traits(FArgs ...)>> +#elif defined _MSC_VER // MS Visual Studio +struct function_traits< + std::_Binder> +#else +#error "Unsupported C++ compiler / standard library" +#endif + : function_traits +{}; + +// std::bind for object const methods +template +#if defined _LIBCPP_VERSION // libc++ (Clang) +struct function_traits> +#elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) +struct function_traits> +#elif defined __GLIBCXX__ // glibc++ (GNU C++) +struct function_traits(FArgs ...)>> +#elif defined _MSC_VER // MS Visual Studio +struct function_traits< + std::_Binder> +#else +#error "Unsupported C++ compiler / standard library" +#endif + : function_traits +{}; + +// std::bind for free functions +template +#if defined _LIBCPP_VERSION // libc++ (Clang) +struct function_traits> +#elif defined __GLIBCXX__ // glibc++ (GNU C++) +struct function_traits> +#elif defined _MSC_VER // MS Visual Studio +struct function_traits> +#else +#error "Unsupported C++ compiler / standard library" +#endif + : function_traits +{}; + +// Lambdas +template +struct function_traits + : function_traits +{}; + +template +struct function_traits: function_traits +{}; + +template +struct function_traits: function_traits +{}; + +/* NOTE(esteve): + * VS2015 does not support expression SFINAE, so we're using this template to evaluate + * the arity of a function. + */ +template +struct arity_comparator : std::integral_constant< + bool, (Arity == function_traits::arity)> {}; + +template +struct check_arguments : std::is_same< + typename function_traits::arguments, + std::tuple +> +{}; + +template +struct same_arguments : std::is_same< + typename function_traits::arguments, + typename function_traits::arguments +> +{}; + +} // namespace function_traits + +} // namespace rclcpp + +#endif // RCLCPP__FUNCTION_TRAITS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/graph_listener.hpp new file mode 100644 index 0000000..ca4f05c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/graph_listener.hpp @@ -0,0 +1,188 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__GRAPH_LISTENER_HPP_ +#define RCLCPP__GRAPH_LISTENER_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl/guard_condition.h" +#include "rcl/wait.h" +#include "rclcpp/context.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +namespace graph_listener +{ + +/// Thrown when a function is called on a GraphListener that is already shutdown. +class GraphListenerShutdownError : public std::runtime_error +{ +public: + GraphListenerShutdownError() + : std::runtime_error("GraphListener already shutdown") {} +}; + +/// Thrown when a node has already been added to the GraphListener. +class NodeAlreadyAddedError : public std::runtime_error +{ +public: + NodeAlreadyAddedError() + : std::runtime_error("node already added") {} +}; + +/// Thrown when the given node is not in the GraphListener. +class NodeNotFoundError : public std::runtime_error +{ +public: + NodeNotFoundError() + : std::runtime_error("node not found") {} +}; + +/// Notifies many nodes of graph changes by listening in a thread. +class GraphListener : public std::enable_shared_from_this +{ +public: + RCLCPP_PUBLIC + explicit GraphListener(std::shared_ptr parent_context); + + RCLCPP_PUBLIC + virtual ~GraphListener(); + + /// Start the graph listener's listen thread if it hasn't been started. + /** + * This function is thread-safe. + * + * \throws GraphListenerShutdownError if the GraphListener is shutdown + */ + RCLCPP_PUBLIC + virtual + void + start_if_not_started(); + + /// Add a node to the graph listener's list of nodes. + /** + * \throws GraphListenerShutdownError if the GraphListener is shutdown + * \throws NodeAlreadyAddedError if the given node is already in the list + * \throws std::invalid_argument if node is nullptr + * \throws std::system_error anything std::mutex::lock() throws + */ + RCLCPP_PUBLIC + virtual + void + add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); + + /// Return true if the given node is in the graph listener's list of nodes. + /** + * Also return false if given nullptr. + * + * \throws std::system_error anything std::mutex::lock() throws + */ + RCLCPP_PUBLIC + virtual + bool + has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); + + /// Remove a node from the graph listener's list of nodes. + /** + * + * \throws NodeNotFoundError if the given node is not in the list + * \throws std::invalid_argument if node is nullptr + * \throws std::system_error anything std::mutex::lock() throws + */ + RCLCPP_PUBLIC + virtual + void + remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); + + /// Stop the listening thread. + /** + * The thread cannot be restarted, and the class is defunct after calling. + * This function is called by the ~GraphListener() and does nothing if + * shutdown() was already called. + * This function exists separately from the ~GraphListener() so that it can + * be called before and exceptions can be caught. + * + * If start_if_not_started() was never called, this function still succeeds, + * but start_if_not_started() still cannot be called after this function. + * + * \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini() + * \throws rclcpp::execptions::RCLError from rcl_wait_set_fini() + * \throws std::system_error anything std::mutex::lock() throws + */ + RCLCPP_PUBLIC + virtual + void + shutdown(); + + /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead. + RCLCPP_PUBLIC + virtual + void + shutdown(const std::nothrow_t &) noexcept; + + /// Return true if shutdown() has been called, else false. + RCLCPP_PUBLIC + virtual + bool + is_shutdown(); + +protected: + /// Main function for the listening thread. + RCLCPP_PUBLIC + virtual + void + run(); + + RCLCPP_PUBLIC + virtual + void + run_loop(); + +private: + RCLCPP_DISABLE_COPY(GraphListener) + + /** \internal */ + void + __shutdown(bool); + + rclcpp::Context::SharedPtr parent_context_; + + std::thread listener_thread_; + bool is_started_; + std::atomic_bool is_shutdown_; + mutable std::mutex shutdown_mutex_; + + mutable std::mutex node_graph_interfaces_barrier_mutex_; + mutable std::mutex node_graph_interfaces_mutex_; + std::vector node_graph_interfaces_; + + rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + std::shared_ptr interrupt_guard_condition_context_; + rcl_guard_condition_t * shutdown_guard_condition_; + rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); +}; + +} // namespace graph_listener +} // namespace rclcpp + +#endif // RCLCPP__GRAPH_LISTENER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/init_options.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/init_options.hpp new file mode 100644 index 0000000..fd0e4f8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/init_options.hpp @@ -0,0 +1,69 @@ +// Copyright 2018 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 RCLCPP__INIT_OPTIONS_HPP_ +#define RCLCPP__INIT_OPTIONS_HPP_ + +#include + +#include "rcl/init_options.h" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Encapsulation of options for initializing rclcpp. +class InitOptions +{ +public: + /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed). + bool shutdown_on_sigint = true; + + /// Constructor which allows you to specify the allocator used within the init options. + RCLCPP_PUBLIC + explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Constructor which is initialized by an existing init_options. + RCLCPP_PUBLIC + explicit InitOptions(const rcl_init_options_t & init_options); + + /// Copy constructor. + RCLCPP_PUBLIC + InitOptions(const InitOptions & other); + + /// Assignment operator. + RCLCPP_PUBLIC + InitOptions & + operator=(const InitOptions & other); + + RCLCPP_PUBLIC + virtual + ~InitOptions(); + + /// Return the rcl init options. + RCLCPP_PUBLIC + const rcl_init_options_t * + get_rcl_init_options() const; + +protected: + void + finalize_init_options(); + +private: + std::unique_ptr init_options_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__INIT_OPTIONS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_buffer_type.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_buffer_type.hpp new file mode 100644 index 0000000..828a96c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_buffer_type.hpp @@ -0,0 +1,35 @@ +// Copyright 2019 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 RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ +#define RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ + +namespace rclcpp +{ + +/// Used as argument in create_publisher and create_subscriber +/// when intra-process communication is enabled +enum class IntraProcessBufferType +{ + /// Set the data type used in the intra-process buffer as std::shared_ptr + SharedPtr, + /// Set the data type used in the intra-process buffer as std::unique_ptr + UniquePtr, + /// Set the data type used in the intra-process buffer as the same used in the callback + CallbackDefault +}; + +} // namespace rclcpp + +#endif // RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_setting.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_setting.hpp new file mode 100644 index 0000000..8e4b44e --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/intra_process_setting.hpp @@ -0,0 +1,34 @@ +// Copyright 2019 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 RCLCPP__INTRA_PROCESS_SETTING_HPP_ +#define RCLCPP__INTRA_PROCESS_SETTING_HPP_ + +namespace rclcpp +{ + +/// Used as argument in create_publisher and create_subscriber. +enum class IntraProcessSetting +{ + /// Explicitly enable intraprocess comm at publisher/subscription level. + Enable, + /// Explicitly disable intraprocess comm at publisher/subscription level. + Disable, + /// Take intraprocess configuration from the node. + NodeDefault +}; + +} // namespace rclcpp + +#endif // RCLCPP__INTRA_PROCESS_SETTING_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/loaned_message.hpp new file mode 100644 index 0000000..ad086cf --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/loaned_message.hpp @@ -0,0 +1,207 @@ +// Copyright 2019 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 RCLCPP__LOANED_MESSAGE_HPP_ +#define RCLCPP__LOANED_MESSAGE_HPP_ + +#include +#include + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/publisher_base.hpp" + +#include "rcl/allocator.h" +#include "rcl/publisher.h" + +namespace rclcpp +{ + +template> +class LoanedMessage +{ + using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; + using MessageAllocator = typename MessageAllocatorTraits::allocator_type; + +public: + /// Constructor of the LoanedMessage class. + /** + * The constructor of this class allocates memory for a given message type + * and associates this with a given publisher. + * + * Given the publisher instance, a case differentiation is being performaned + * which decides whether the underlying middleware is able to allocate the appropriate + * memory for this message type or not. + * In the case that the middleware can not loan messages, the passed in allocator instance + * is being used to allocate the message within the scope of this class. + * Otherwise, the allocator is being ignored and the allocation is solely performaned + * in the underlying middleware with its appropriate allocation strategy. + * The need for this arises as the user code can be written explicitly targeting a middleware + * capable of loaning messages. + * However, this user code is ought to be usable even when dynamically linked against + * a middleware which doesn't support message loaning in which case the allocator will be used. + * + * \param pub rclcpp::Publisher instance to which the memory belongs + * \param allocator Allocator instance in case middleware can not allocate messages + */ + LoanedMessage( + const rclcpp::PublisherBase & pub, + std::allocator allocator) + : pub_(pub), + message_(nullptr), + message_allocator_(std::move(allocator)) + { + if (pub_.can_loan_messages()) { + void * message_ptr = nullptr; + auto ret = rcl_borrow_loaned_message( + pub_.get_publisher_handle(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + &message_ptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + message_ = static_cast(message_ptr); + } else { + RCLCPP_INFO_ONCE( + rclcpp::get_logger("rclcpp"), + "Currently used middleware can't loan messages. Local allocator will be used."); + message_ = message_allocator_.allocate(1); + new (message_) MessageT(); + } + } + + /// Constructor of the LoanedMessage class. + /** + * The constructor of this class allocates memory for a given message type + * and associates this with a given publisher. + * + * Given the publisher instance, a case differentiation is being performaned + * which decides whether the underlying middleware is able to allocate the appropriate + * memory for this message type or not. + * In the case that the middleware can not loan messages, the passed in allocator instance + * is being used to allocate the message within the scope of this class. + * Otherwise, the allocator is being ignored and the allocation is solely performaned + * in the underlying middleware with its appropriate allocation strategy. + * The need for this arises as the user code can be written explicitly targeting a middleware + * capable of loaning messages. + * However, this user code is ought to be usable even when dynamically linked against + * a middleware which doesn't support message loaning in which case the allocator will be used. + * + * \param pub rclcpp::Publisher instance to which the memory belongs + * \param allocator Allocator instance in case middleware can not allocate messages + */ + LoanedMessage( + const rclcpp::PublisherBase * pub, + std::shared_ptr> allocator) + : LoanedMessage(*pub, *allocator) + {} + + /// Move semantic for RVO + LoanedMessage(LoanedMessage && other) + : pub_(std::move(other.pub_)), + message_(std::move(other.message_)), + message_allocator_(std::move(other.message_allocator_)) + {} + + /// Destructor of the LoanedMessage class. + /** + * The destructor has the explicit task to return the allocated memory for its message + * instance. + * If the message was previously allocated via the middleware, the message is getting + * returned to the middleware to cleanly destroy the allocation. + * In the case that the local allocator instance was used, the same instance is then + * being used to destroy the allocated memory. + * + * The contract here is that the memory for this message is valid as long as this instance + * of the LoanedMessage class is alive. + */ + virtual ~LoanedMessage() + { + auto error_logger = rclcpp::get_logger("LoanedMessage"); + + if (message_ == nullptr) { + return; + } + + if (pub_.can_loan_messages()) { + // return allocated memory to the middleware + auto ret = + rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle(), message_); + if (ret != RCL_RET_OK) { + RCLCPP_ERROR( + error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + // call destructor before deallocating + message_->~MessageT(); + message_allocator_.deallocate(message_, 1); + } + message_ = nullptr; + } + + /// Validate if the message was correctly allocated. + /** + * The allocated memory might not be always consistent and valid. + * Reasons why this could fail is that an allocation step was failing, + * e.g. just like malloc could fail or a maximum amount of previously allocated + * messages is exceeded in which case the loaned messages have to be returned + * to the middleware prior to be able to allocate a new one. + */ + bool is_valid() const + { + return message_ != nullptr; + } + + /// Access the ROS message instance. + /** + * A call to `get()` will return a mutable reference to the underlying ROS message instance. + * This allows a user to modify the content of the message prior to publishing it. + * + * If this reference is copied, the memory for this copy is no longer managed + * by the LoanedMessage instance and has to be cleanup individually. + */ + MessageT & get() const + { + return *message_; + } + + /// Release ownership of the ROS message instance. + /** + * A call to `release()` will unmanage the memory for the ROS message. + * That means that the destructor of this class will not free the memory on scope exit. + * + * \return Raw pointer to the message instance. + */ + MessageT * release() + { + auto msg = message_; + message_ = nullptr; + return msg; + } + +protected: + const rclcpp::PublisherBase & pub_; + + MessageT * message_; + + MessageAllocator message_allocator_; + + /// Deleted copy constructor to preserve memory integrity. + LoanedMessage(const LoanedMessage & other) = delete; +}; + +} // namespace rclcpp + +#endif // RCLCPP__LOANED_MESSAGE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/logger.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/logger.hpp new file mode 100644 index 0000000..7e7049d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/logger.hpp @@ -0,0 +1,145 @@ +// Copyright 2017 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 RCLCPP__LOGGER_HPP_ +#define RCLCPP__LOGGER_HPP_ + +#include +#include + +#include "rclcpp/visibility_control.hpp" + +#include "rcl/node.h" + +/** + * \def RCLCPP_LOGGING_ENABLED + * When this define evaluates to true (default), logger factory functions will + * behave normally. + * When false, logger factory functions will create dummy loggers to avoid + * computational expense in manipulating objects. + * This should be used in combination with `RCLCPP_LOG_MIN_SEVERITY` to compile + * out logging macros. + */ +// TODO(dhood): determine this automatically from `RCLCPP_LOG_MIN_SEVERITY` +#ifndef RCLCPP_LOGGING_ENABLED +#define RCLCPP_LOGGING_ENABLED 1 +#endif + +namespace rclcpp +{ + +// Forward declaration is used for friend statement. +namespace node_interfaces +{ +class NodeLogging; +} + +class Logger; + +/// Return a named logger. +/** + * The returned logger's name will include any naming conventions, such as a + * name prefix. + * Currently there are no such naming conventions but they may be introduced in + * the future. + * + * \param[in] name the name of the logger + * \return a logger with the fully-qualified name including naming conventions, or + * \return a dummy logger if logging is disabled. + */ +RCLCPP_PUBLIC +Logger +get_logger(const std::string & name); + +/// Return a named logger using an rcl_node_t. +/** + * This is a convenience function that does error checking and returns the node + * logger name, or "rclcpp" if it is unable to get the node name. + * + * \param[in] node the rcl node from which to get the logger name + * \return a logger based on the node name, or "rclcpp" if there's an error + */ +RCLCPP_PUBLIC +Logger +get_node_logger(const rcl_node_t * node); + +class Logger +{ +private: + friend Logger rclcpp::get_logger(const std::string & name); + friend ::rclcpp::node_interfaces::NodeLogging; + + /// Constructor of a dummy logger. + /** + * This is used when logging is disabled: see `RCLCPP_LOGGING_ENABLED`. + * This cannot be called directly, see `rclcpp::get_logger` instead. + */ + Logger() + : name_(nullptr) {} + + /// Constructor of a named logger. + /** + * This cannot be called directly, see `rclcpp::get_logger` instead. + */ + explicit Logger(const std::string & name) + : name_(new std::string(name)) {} + + std::shared_ptr name_; + +public: + RCLCPP_PUBLIC + Logger(const Logger &) = default; + + /// Get the name of this logger. + /** + * \return the full name of the logger including any prefixes, or + * \return `nullptr` if this logger is invalid (e.g. because logging is + * disabled). + */ + RCLCPP_PUBLIC + const char * + get_name() const + { + if (!name_) { + return nullptr; + } + return name_->c_str(); + } + + /// Return a logger that is a descendant of this logger. + /** + * The child logger's full name will include any hierarchy conventions that + * indicate it is a descendant of this logger. + * For example, ```get_logger('abc').get_child('def')``` will return a logger + * with name `abc.def`. + * + * \param[in] suffix the child logger's suffix + * \return a logger with the fully-qualified name including the suffix, or + * \return a dummy logger if this logger is invalid (e.g. because logging is + * disabled). + */ + RCLCPP_PUBLIC + Logger + get_child(const std::string & suffix) + { + if (!name_) { + return Logger(); + } + return Logger(*name_ + "." + suffix); + } +}; + +} // namespace rclcpp + +#endif // RCLCPP__LOGGER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/macros.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/macros.hpp new file mode 100644 index 0000000..d13c5d2 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/macros.hpp @@ -0,0 +1,114 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#ifndef RCLCPP__MACROS_HPP_ +#define RCLCPP__MACROS_HPP_ + +/** + * Disables the copy constructor and operator= for the given class. + * + * Use in the private section of the class. + */ +#define RCLCPP_DISABLE_COPY(...) \ + __VA_ARGS__(const __VA_ARGS__ &) = delete; \ + __VA_ARGS__ & operator=(const __VA_ARGS__ &) = delete; + +/** + * Defines aliases and static functions for using the Class with smart pointers. + * + * Use in the public section of the class. + * Make sure to include `` in the header when using this. + */ +#define RCLCPP_SMART_PTR_DEFINITIONS(...) \ + RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \ + RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ + RCLCPP_UNIQUE_PTR_DEFINITIONS(__VA_ARGS__) + +/** + * Defines aliases and static functions for using the Class with smart pointers. + * + * Same as RCLCPP_SMART_PTR_DEFINITIONS except it excludes the static + * Class::make_unique() method definition which does not work on classes which + * are not CopyConstructable. + * + * Use in the public section of the class. + * Make sure to include `` in the header when using this. + */ +#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...) \ + RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \ + RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ + __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) + +/** + * Defines aliases only for using the Class with smart pointers. + * + * Same as RCLCPP_SMART_PTR_DEFINITIONS except it excludes the static + * method definitions which do not work on pure virtual classes and classes + * which are not CopyConstructable. + * + * Use in the public section of the class. + * Make sure to include `` in the header when using this. + */ +#define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \ + __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) + +#define __RCLCPP_SHARED_PTR_ALIAS(...) \ + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ + using ConstSharedPtr = std::shared_ptr; + +#define __RCLCPP_MAKE_SHARED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ + } + +/// Defines aliases and static functions for using the Class with shared_ptrs. +#define RCLCPP_SHARED_PTR_DEFINITIONS(...) \ + __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) + +#define __RCLCPP_WEAK_PTR_ALIAS(...) \ + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ + using ConstWeakPtr = std::weak_ptr; + +/// Defines aliases and static functions for using the Class with weak_ptrs. +#define RCLCPP_WEAK_PTR_DEFINITIONS(...) __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) + +#define __RCLCPP_UNIQUE_PTR_ALIAS(...) using UniquePtr = std::unique_ptr<__VA_ARGS__>; + +#define __RCLCPP_MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ + } + +/// Defines aliases and static functions for using the Class with unique_ptrs. +#define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \ + __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + +#define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2) +#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2 + +#endif // RCLCPP__MACROS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategies.hpp new file mode 100644 index 0000000..a3950d9 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategies.hpp @@ -0,0 +1,33 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__MEMORY_STRATEGIES_HPP_ +#define RCLCPP__MEMORY_STRATEGIES_HPP_ + +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace memory_strategies +{ + +RCLCPP_PUBLIC +memory_strategy::MemoryStrategy::SharedPtr +create_default_strategy(); + +} // namespace memory_strategies +} // namespace rclcpp + +#endif // RCLCPP__MEMORY_STRATEGIES_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategy.hpp new file mode 100644 index 0000000..aaf730b --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/memory_strategy.hpp @@ -0,0 +1,149 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__MEMORY_STRATEGY_HPP_ +#define RCLCPP__MEMORY_STRATEGY_HPP_ + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/wait.h" + +#include "rclcpp/any_executable.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace memory_strategy +{ + +/// Delegate for handling memory allocations while the Executor is executing. +/** + * By default, the memory strategy dynamically allocates memory for structures that come in from + * the rmw implementation after the executor waits for work, based on the number of entities that + * come through. + */ +class RCLCPP_PUBLIC MemoryStrategy +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy) + using WeakNodeList = std::list; + + virtual ~MemoryStrategy() = default; + + virtual bool collect_entities(const WeakNodeList & weak_nodes) = 0; + + virtual size_t number_of_ready_subscriptions() const = 0; + virtual size_t number_of_ready_services() const = 0; + virtual size_t number_of_ready_clients() const = 0; + virtual size_t number_of_ready_events() const = 0; + virtual size_t number_of_ready_timers() const = 0; + virtual size_t number_of_guard_conditions() const = 0; + virtual size_t number_of_waitables() const = 0; + + virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0; + virtual void clear_handles() = 0; + virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; + + virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + + virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + + virtual void + get_next_subscription( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) = 0; + + virtual void + get_next_service( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) = 0; + + virtual void + get_next_client( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) = 0; + + virtual void + get_next_timer( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) = 0; + + virtual void + get_next_waitable( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) = 0; + + virtual rcl_allocator_t + get_allocator() = 0; + + static rclcpp::SubscriptionBase::SharedPtr + get_subscription_by_handle( + std::shared_ptr subscriber_handle, + const WeakNodeList & weak_nodes); + + static rclcpp::ServiceBase::SharedPtr + get_service_by_handle( + std::shared_ptr service_handle, + const WeakNodeList & weak_nodes); + + static rclcpp::ClientBase::SharedPtr + get_client_by_handle( + std::shared_ptr client_handle, + const WeakNodeList & weak_nodes); + + static rclcpp::TimerBase::SharedPtr + get_timer_by_handle( + std::shared_ptr timer_handle, + const WeakNodeList & weak_nodes); + + static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_by_group( + rclcpp::callback_group::CallbackGroup::SharedPtr group, + const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription, + const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_service( + rclcpp::ServiceBase::SharedPtr service, + const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_client( + rclcpp::ClientBase::SharedPtr client, + const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_timer( + rclcpp::TimerBase::SharedPtr timer, + const WeakNodeList & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_waitable( + rclcpp::Waitable::SharedPtr waitable, + const WeakNodeList & weak_nodes); +}; + +} // namespace memory_strategy +} // namespace rclcpp + +#endif // RCLCPP__MEMORY_STRATEGY_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/message_memory_strategy.hpp new file mode 100644 index 0000000..80654d7 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -0,0 +1,151 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_ +#define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_ + +#include +#include + +#include "rcl/types.h" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcutils/logging_macros.h" + +#include "rmw/serialized_message.h" + +namespace rclcpp +{ +namespace message_memory_strategy +{ + +/// Default allocation strategy for messages received by subscriptions. +/** A message memory strategy must be templated on the type of the subscription it belongs to. */ +template> +class MessageMemoryStrategy +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy) + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + + using SerializedMessageAllocTraits = allocator::AllocRebind; + using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type; + using SerializedMessageDeleter = + allocator::Deleter; + + using BufferAllocTraits = allocator::AllocRebind; + using BufferAlloc = typename BufferAllocTraits::allocator_type; + using BufferDeleter = allocator::Deleter; + + MessageMemoryStrategy() + { + message_allocator_ = std::make_shared(); + serialized_message_allocator_ = std::make_shared(); + buffer_allocator_ = std::make_shared(); + rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); + } + + explicit MessageMemoryStrategy(std::shared_ptr allocator) + { + message_allocator_ = std::make_shared(*allocator.get()); + serialized_message_allocator_ = std::make_shared(*allocator.get()); + buffer_allocator_ = std::make_shared(*allocator.get()); + rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); + } + + virtual ~MessageMemoryStrategy() = default; + + /// Default factory method + static SharedPtr create_default() + { + return std::make_shared>(std::make_shared()); + } + + /// By default, dynamically allocate a new message. + /** \return Shared pointer to the new message. */ + virtual std::shared_ptr borrow_message() + { + return std::allocate_shared(*message_allocator_.get()); + } + + virtual std::shared_ptr borrow_serialized_message(size_t capacity) + { + auto msg = new rcl_serialized_message_t; + *msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + auto serialized_msg = std::shared_ptr( + msg, + [](rmw_serialized_message_t * msg) { + auto fini_ret = rmw_serialized_message_fini(msg); + delete msg; + if (fini_ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy serialized message: %s", rcl_get_error_string().str); + } + }); + + return serialized_msg; + } + + virtual std::shared_ptr borrow_serialized_message() + { + return borrow_serialized_message(default_buffer_capacity_); + } + + virtual void set_default_buffer_capacity(size_t capacity) + { + default_buffer_capacity_ = capacity; + } + + /// Release ownership of the message, which will deallocate it if it has no more owners. + /** \param[in] msg Shared pointer to the message we are returning. */ + virtual void return_message(std::shared_ptr & msg) + { + msg.reset(); + } + + virtual void return_serialized_message(std::shared_ptr & serialized_msg) + { + serialized_msg.reset(); + } + + std::shared_ptr message_allocator_; + MessageDeleter message_deleter_; + + std::shared_ptr serialized_message_allocator_; + SerializedMessageDeleter serialized_message_deleter_; + + std::shared_ptr buffer_allocator_; + BufferDeleter buffer_deleter_; + size_t default_buffer_capacity_ = 0; + + rcutils_allocator_t rcutils_allocator_; +}; + +} // namespace message_memory_strategy +} // namespace rclcpp + +#endif // RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node.hpp new file mode 100644 index 0000000..295bf91 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node.hpp @@ -0,0 +1,1117 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_HPP_ +#define RCLCPP__NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcutils/macros.h" + +#include "rcl/error_handling.h" +#include "rcl/node.h" + +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Node is the single point of entry for creating publishers and subscribers. +class Node : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Node) + + /// Create a new node with the specified name. + /** + * \param[in] node_name Name of the node. + * \param[in] options Additional options to control creation of the node. + */ + RCLCPP_PUBLIC + explicit Node( + const std::string & node_name, + const NodeOptions & options = NodeOptions()); + + /// Create a new node with the specified name. + /** + * \param[in] node_name Name of the node. + * \param[in] namespace_ Namespace of the node. + * \param[in] options Additional options to control creation of the node. + */ + RCLCPP_PUBLIC + explicit Node( + const std::string & node_name, + const std::string & namespace_, + const NodeOptions & options = NodeOptions()); + + RCLCPP_PUBLIC + virtual ~Node(); + + /// Get the name of the node. + /** \return The name of the node. */ + RCLCPP_PUBLIC + const char * + get_name() const; + + /// Get the namespace of the node. + /** + * This namespace is the "node's" namespace, and therefore is not affected + * by any sub-namespace's that may affect entities created with this instance. + * Use get_effective_namespace() to get the full namespace used by entities. + * + * \sa get_sub_namespace() + * \sa get_effective_namespace() + * \return The namespace of the node. + */ + RCLCPP_PUBLIC + const char * + get_namespace() const; + + /// Get the fully-qualified name of the node. + /** + * The fully-qualified name includes the local namespace and name of the node. + */ + RCLCPP_PUBLIC + const char * + get_fully_qualified_name() const; + + /// Get the logger of the node. + /** \return The logger of the node. */ + RCLCPP_PUBLIC + rclcpp::Logger + get_logger() const; + + /// Create and return a callback group. + RCLCPP_PUBLIC + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + + /// Return the list of callback groups in the node. + RCLCPP_PUBLIC + const std::vector & + get_callback_groups() const; + + /// Create and return a Publisher. + /** + * The rclcpp::QoS has several convenient constructors, including a + * conversion constructor for size_t, which mimics older API's that + * allows just a string and size_t to create a publisher. + * + * For example, all of these cases will work: + * + * ```cpp + * pub = node->create_publisher("chatter", 10); // implicitly KeepLast + * pub = node->create_publisher("chatter", QoS(10)); // implicitly KeepLast + * pub = node->create_publisher("chatter", QoS(KeepLast(10))); + * pub = node->create_publisher("chatter", QoS(KeepAll())); + * pub = node->create_publisher("chatter", QoS(1).best_effort().volatile()); + * { + * rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data); + * pub = node->create_publisher("chatter", custom_qos); + * } + * ``` + * + * The publisher options may optionally be passed as the third argument for + * any of the above cases. + * + * \param[in] topic_name The topic for this publisher to publish on. + * \param[in] qos The Quality of Service settings for the publisher. + * \param[in] options Additional options for the created Publisher. + * \return Shared pointer to the created publisher. + */ + template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher> + std::shared_ptr + create_publisher( + const std::string & topic_name, + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options = + PublisherOptionsWithAllocator() + ); + + /// Create and return a Subscription. + /** + * \param[in] topic_name The topic to subscribe on. + * \param[in] callback The user-defined callback function to receive a message + * \param[in] qos_history_depth The depth of the subscription's incoming message queue. + * \param[in] options Additional options for the creation of the Subscription. + * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. + * \return Shared pointer to the created subscription. + */ + template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + AllocatorT + > + > + std::shared_ptr + create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const SubscriptionOptionsWithAllocator & options = + SubscriptionOptionsWithAllocator(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) + ); + + /// Create a timer. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::WallTimer::SharedPtr + create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /* Create and return a Client. */ + template + typename rclcpp::Client::SharedPtr + create_client( + const std::string & service_name, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /* Create and return a Service. */ + template + typename rclcpp::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /// Declare and initialize a parameter, return the effective value. + /** + * This method is used to declare that a parameter exists on this node. + * If, at run-time, the user has provided an initial value then it will be + * set in this method, otherwise the given default_value will be set. + * In either case, the resulting value is returned, whether or not it is + * based on the default value or the user provided initial value. + * + * If no parameter_descriptor is given, then the default values from the + * message definition will be used, e.g. read_only will be false. + * + * The name and type in the given rcl_interfaces::msg::ParameterDescriptor + * are ignored, and should be specified using the name argument to this + * function and the default value's type instead. + * + * If `ignore_override` is `true`, the parameter override will be ignored. + * + * This method, if successful, will result in any callback registered with + * set_on_parameters_set_callback to be called. + * If that callback prevents the initial value for the parameter from being + * set then rclcpp::exceptions::InvalidParameterValueException is thrown. + * + * The returned reference will remain valid until the parameter is + * undeclared. + * + * \param[in] name The name of the parameter. + * \param[in] default_value An initial value to be used if at run-time user + * did not override it. + * \param[in] parameter_descriptor An optional, custom description for + * the parameter. + * \param[in] ignore_override When `true`, the parameter override is ignored. + * Default to `false`. + * \return A const reference to the value of the parameter. + * \throws rclcpp::exceptions::ParameterAlreadyDeclaredException if parameter + * has already been declared. + * \throws rclcpp::exceptions::InvalidParametersException if a parameter + * name is invalid. + * \throws rclcpp::exceptions::InvalidParameterValueException if initial + * value fails to be set. + */ + RCLCPP_PUBLIC + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + + /// Declare and initialize a parameter with a type. + /** + * See the non-templated declare_parameter() on this class for details. + * + * If the type of the default value, and therefore also the type of return + * value, differs from the initial value provided in the node options, then + * a rclcpp::ParameterTypeException may be thrown. + * To avoid this, use the declare_parameter() method which returns an + * rclcpp::ParameterValue instead. + * + * Note, this method cannot return a const reference, because extending the + * lifetime of a temporary only works recursively with member initializers, + * and cannot be extended to members of a class returned. + * The return value of this class is a copy of the member of a ParameterValue + * which is returned by the other version of declare_parameter(). + * See also: + * + * - https://en.cppreference.com/w/cpp/language/lifetime + * - https://herbsutter.com/2008/01/01/gotw-88-a-candidate-for-the-most-important-const/ + * - https://www.youtube.com/watch?v=uQyT-5iWUow (cppnow 2018 presentation) + */ + template + auto + declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + + /// Declare and initialize several parameters with the same namespace and type. + /** + * For each key in the map, a parameter with a name of "namespace.key" + * will be set to the value in the map. + * The resulting value for each declared parameter will be returned. + * + * The name expansion is naive, so if you set the namespace to be "foo.", + * then the resulting parameter names will be like "foo..key". + * However, if the namespace is an empty string, then no leading '.' will be + * placed before each key, which would have been the case when naively + * expanding "namespace.key". + * This allows you to declare several parameters at once without a namespace. + * + * The map contains default values for parameters. + * There is another overload which takes the std::pair with the default value + * and descriptor. + * + * If `ignore_overrides` is `true`, all the overrides of the parameters declared + * by the function call will be ignored. + * + * This method, if successful, will result in any callback registered with + * set_on_parameters_set_callback to be called, once for each parameter. + * If that callback prevents the initial value for any parameter from being + * set then rclcpp::exceptions::InvalidParameterValueException is thrown. + * + * \param[in] namespace_ The namespace in which to declare the parameters. + * \param[in] parameters The parameters to set in the given namespace. + * \param[in] ignore_overrides When `true`, the parameters overrides are ignored. + * Default to `false`. + * \throws rclcpp::exceptions::ParameterAlreadyDeclaredException if parameter + * has already been declared. + * \throws rclcpp::exceptions::InvalidParametersException if a parameter + * name is invalid. + * \throws rclcpp::exceptions::InvalidParameterValueException if initial + * value fails to be set. + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map & parameters, + bool ignore_overrides = false); + + /// Declare and initialize several parameters with the same namespace and type. + /** + * This version will take a map where the value is a pair, with the default + * parameter value as the first item and a parameter descriptor as the second. + * + * See the simpler declare_parameters() on this class for more details. + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters, + bool ignore_overrides = false); + + /// Undeclare a previously declared parameter. + /** + * This method will not cause a callback registered with + * set_on_parameters_set_callback to be called. + * + * \param[in] name The name of the parameter to be undeclared. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter + * has not been declared. + * \throws rclcpp::exceptions::ParameterImmutableException if the parameter + * was create as read_only (immutable). + */ + RCLCPP_PUBLIC + void + undeclare_parameter(const std::string & name); + + /// Return true if a given parameter is declared. + /** + * \param[in] name The name of the parameter to check for being declared. + * \return true if the parameter name has been declared, otherwise false. + */ + RCLCPP_PUBLIC + bool + has_parameter(const std::string & name) const; + + /// Set a single parameter. + /** + * Set the given parameter and then return result of the set action. + * + * If the parameter has not been declared this function may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception, but only if + * the node was not created with the + * rclcpp::NodeOptions::allow_undeclared_parameters set to true. + * If undeclared parameters are allowed, then the parameter is implicitly + * declared with the default parameter meta data before being set. + * Parameter overrides are ignored by set_parameter. + * + * This method will result in any callback registered with + * set_on_parameters_set_callback to be called. + * If the callback prevents the parameter from being set, then it will be + * reflected in the SetParametersResult that is returned, but no exception + * will be thrown. + * + * If the value type of the parameter is rclcpp::PARAMETER_NOT_SET, and the + * existing parameter type is something else, then the parameter will be + * implicitly undeclared. + * This will result in a parameter event indicating that the parameter was + * deleted. + * + * \param[in] parameter The parameter to be set. + * \return The result of the set action. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter + * has not been declared and undeclared parameters are not allowed. + */ + RCLCPP_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameter(const rclcpp::Parameter & parameter); + + /// Set one or more parameters, one at a time. + /** + * Set the given parameters, one at a time, and then return result of each + * set action. + * + * Parameters are set in the order they are given within the input vector. + * + * Like set_parameter, if any of the parameters to be set have not first been + * declared, and undeclared parameters are not allowed (the default), then + * this method will throw rclcpp::exceptions::ParameterNotDeclaredException. + * + * If setting a parameter fails due to not being declared, then the + * parameters which have already been set will stay set, and no attempt will + * be made to set the parameters which come after. + * + * If a parameter fails to be set due to any other reason, like being + * rejected by the user's callback (basically any reason other than not + * having been declared beforehand), then that is reflected in the + * corresponding SetParametersResult in the vector returned by this function. + * + * This method will result in any callback registered with + * set_on_parameters_set_callback to be called, once for each parameter. + * If the callback prevents the parameter from being set, then, as mentioned + * before, it will be reflected in the corresponding SetParametersResult + * that is returned, but no exception will be thrown. + * + * Like set_parameter() this method will implicitly undeclare parameters + * with the type rclcpp::PARAMETER_NOT_SET. + * + * \param[in] parameters The vector of parameters to be set. + * \return The results for each set action as a vector. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any parameter + * has not been declared and undeclared parameters are not allowed. + */ + RCLCPP_PUBLIC + std::vector + set_parameters(const std::vector & parameters); + + /// Set one or more parameters, all at once. + /** + * Set the given parameters, all at one time, and then aggregate result. + * + * Behaves like set_parameter, except that it sets multiple parameters, + * failing all if just one of the parameters are unsuccessfully set. + * Either all of the parameters are set or none of them are set. + * + * Like set_parameter and set_parameters, this method may throw an + * rclcpp::exceptions::ParameterNotDeclaredException exception if any of the + * parameters to be set have not first been declared. + * If the exception is thrown then none of the parameters will have been set. + * + * This method will result in any callback registered with + * set_on_parameters_set_callback to be called, just one time. + * If the callback prevents the parameters from being set, then it will be + * reflected in the SetParametersResult which is returned, but no exception + * will be thrown. + * + * If you pass multiple rclcpp::Parameter instances with the same name, then + * only the last one in the vector (forward iteration) will be set. + * + * Like set_parameter() this method will implicitly undeclare parameters + * with the type rclcpp::PARAMETER_NOT_SET. + * + * \param[in] parameters The vector of parameters to be set. + * \return The aggregate result of setting all the parameters atomically. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any parameter + * has not been declared and undeclared parameters are not allowed. + */ + RCLCPP_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically(const std::vector & parameters); + + /// Return the parameter by the given name. + /** + * If the parameter has not been declared, then this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception. + * + * If undeclared parameters are allowed, see the node option + * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will + * not throw an exception, and instead return a default initialized + * rclcpp::Parameter, which has a type of + * rclcpp::ParameterType::PARAMETER_NOT_SET. + * + * \param[in] name The name of the parameter to get. + * \return The requested parameter inside of a rclcpp parameter object. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter + * has not been declared and undeclared parameters are not allowed. + */ + RCLCPP_PUBLIC + rclcpp::Parameter + get_parameter(const std::string & name) const; + + /// Get the value of a parameter by the given name, and return true if it was set. + /** + * This method will never throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception, but will + * instead return false if the parameter has not be previously declared. + * + * If the parameter was not declared, then the output argument for this + * method which is called "parameter" will not be assigned a value. + * If the parameter was declared, and therefore has a value, then it is + * assigned into the "parameter" argument of this method. + * + * \param[in] name The name of the parameter to get. + * \param[out] parameter The output storage for the parameter being retrieved. + * \return true if the parameter was previously declared, otherwise false. + */ + RCLCPP_PUBLIC + bool + get_parameter(const std::string & name, rclcpp::Parameter & parameter) const; + + /// Get the value of a parameter by the given name, and return true if it was set. + /** + * Identical to the non-templated version of this method, except that when + * assigning the output argument called "parameter", this method will attempt + * to coerce the parameter value into the type requested by the given + * template argument, which may fail and throw an exception. + * + * If the parameter has not been declared, it will not attempt to coerce the + * value into the requested type, as it is known that the type is not set. + * + * \throws rclcpp::ParameterTypeException if the requested type does not + * match the value of the parameter which is stored. + */ + template + bool + get_parameter(const std::string & name, ParameterT & parameter) const; + + /// Get the parameter value, or the "alternative_value" if not set, and assign it to "parameter". + /** + * If the parameter was not set, then the "parameter" argument is assigned + * the "alternative_value". + * + * Like the version of get_parameter() which returns a bool, this method will + * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception. + * + * In all cases, the parameter is never set or declared within the node. + * + * \param[in] name The name of the parameter to get. + * \param[out] parameter The output where the value of the parameter should be assigned. + * \param[in] alternative_value Value to be stored in output if the parameter was not set. + * \returns true if the parameter was set, false otherwise. + */ + template + bool + get_parameter_or( + const std::string & name, + ParameterT & parameter, + const ParameterT & alternative_value) const; + + /// Return the parameters by the given parameter names. + /** + * Like get_parameters(), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if the + * requested parameter has not been declared and undeclared parameters are + * not allowed. + * + * Also like get_parameters(), if undeclared parameters are allowed and the + * parameter has not been declared, then the corresponding rclcpp::Parameter + * will be default initialized and therefore have the type + * rclcpp::ParameterType::PARAMETER_NOT_SET. + * + * \param[in] names The names of the parameters to be retrieved. + * \return The parameters that were retrieved. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the + * parameters have not been declared and undeclared parameters are not + * allowed. + */ + RCLCPP_PUBLIC + std::vector + get_parameters(const std::vector & names) const; + + /// Get the parameter values for all parameters that have a given prefix. + /** + * The "prefix" argument is used to list the parameters which are prefixed + * with that prefix, see also list_parameters(). + * + * The resulting list of parameter names are used to get the values of the + * parameters. + * + * The names which are used as keys in the values map have the prefix removed. + * For example, if you use the prefix "foo" and the parameters "foo.ping" and + * "foo.pong" exist, then the returned map will have the keys "ping" and + * "pong". + * + * An empty string for the prefix will match all parameters. + * + * If no parameters with the prefix are found, then the output parameter + * "values" will be unchanged and false will be returned. + * Otherwise, the parameter names and values will be stored in the map and + * true will be returned to indicate "values" was mutated. + * + * This method will never throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception because the + * action of listing the parameters is done atomically with getting the + * values, and therefore they are only listed if already declared and cannot + * be undeclared before being retrieved. + * + * Like the templated get_parameter() variant, this method will attempt to + * coerce the parameter values into the type requested by the given + * template argument, which may fail and throw an exception. + * + * \param[in] prefix The prefix of the parameters to get. + * \param[out] values The map used to store the parameter names and values, + * respectively, with one entry per parameter matching prefix. + * \returns true if output "values" was changed, false otherwise. + * \throws rclcpp::ParameterTypeException if the requested type does not + * match the value of the parameter which is stored. + */ + template + bool + get_parameters( + const std::string & prefix, + std::map & values) const; + + /// Return the parameter descriptor for the given parameter name. + /** + * Like get_parameters(), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if the + * requested parameter has not been declared and undeclared parameters are + * not allowed. + * + * If undeclared parameters are allowed, then a default initialized + * descriptor will be returned. + * + * \param[in] name The name of the parameter to describe. + * \return The descriptor for the given parameter name. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the + * parameter has not been declared and undeclared parameters are not + * allowed. + */ + RCLCPP_PUBLIC + rcl_interfaces::msg::ParameterDescriptor + describe_parameter(const std::string & name) const; + + /// Return a vector of parameter descriptors, one for each of the given names. + /** + * Like get_parameters(), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if any of the + * requested parameters have not been declared and undeclared parameters are + * not allowed. + * + * If undeclared parameters are allowed, then a default initialized + * descriptor will be returned for the undeclared parameter's descriptor. + * + * If the names vector is empty, then an empty vector will be returned. + * + * \param[in] names The list of parameter names to describe. + * \return A list of parameter descriptors, one for each parameter given. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the + * parameters have not been declared and undeclared parameters are not + * allowed. + */ + RCLCPP_PUBLIC + std::vector + describe_parameters(const std::vector & names) const; + + /// Return a vector of parameter types, one for each of the given names. + /** + * Like get_parameters(), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if any of the + * requested parameters have not been declared and undeclared parameters are + * not allowed. + * + * If undeclared parameters are allowed, then the default type + * rclcpp::ParameterType::PARAMETER_NOT_SET will be returned. + * + * \param[in] names The list of parameter names to get the types. + * \return A list of parameter types, one for each parameter given. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the + * parameters have not been declared and undeclared parameters are not + * allowed. + */ + RCLCPP_PUBLIC + std::vector + get_parameter_types(const std::vector & names) const; + + /// Return a list of parameters with any of the given prefixes, up to the given depth. + /** + * \todo: properly document and test this method. + */ + RCLCPP_PUBLIC + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const; + + using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; + using OnParametersSetCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + + /// Add a callback for when parameters are being set. + /** + * The callback signature is designed to allow handling of any of the above + * `set_parameter*` or `declare_parameter*` methods, and so it takes a const + * reference to a vector of parameters to be set, and returns an instance of + * rcl_interfaces::msg::SetParametersResult to indicate whether or not the + * parameter should be set or not, and if not why. + * + * For an example callback: + * + * ```cpp + * rcl_interfaces::msg::SetParametersResult + * my_callback(const std::vector & parameters) + * { + * rcl_interfaces::msg::SetParametersResult result; + * result.successful = true; + * for (const auto & parameter : parameters) { + * if (!some_condition) { + * result.successful = false; + * result.reason = "the reason it could not be allowed"; + * } + * } + * return result; + * } + * ``` + * + * You can see that the SetParametersResult is a boolean flag for success + * and an optional reason that can be used in error reporting when it fails. + * + * This allows the node developer to control which parameters may be changed. + * + * Note that the callback is called when declare_parameter() and its variants + * are called, and so you cannot assume the parameter has been set before + * this callback, so when checking a new value against the existing one, you + * must account for the case where the parameter is not yet set. + * + * Some constraints like read_only are enforced before the callback is called. + * + * The callback may introspect other already set parameters (by calling any + * of the {get,list,describe}_parameter() methods), but may *not* modify + * other parameters (by calling any of the {set,declare}_parameter() methods) + * or modify the registered callback itself (by calling the + * set_on_parameters_set_callback() method). If a callback tries to do any + * of the latter things, + * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown. + * + * The callback functions must remain valid as long as the + * returned smart pointer is valid. + * The returned smart pointer can be promoted to a shared version. + * + * Resetting or letting the smart pointer go out of scope unregisters the callback. + * `remove_on_set_parameters_callback` can also be used. + * + * The registered callbacks are called when a parameter is set. + * When a callback returns a not successful result, the remaining callbacks aren't called. + * The order of the callback is the reverse from the registration order. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnParametersSetCallbackType callback); + + /// Remove a callback registered with `add_on_set_parameters_callback`. + /** + * Delete a handler returned by `add_on_set_parameters_callback`. + * + * e.g.: + * + * `remove_on_set_parameters_callback(scoped_callback.get())` + * + * As an alternative, the smart pointer can be reset: + * + * `scoped_callback.reset()` + * + * Supposing that `scoped_callback` was the only owner. + * + * Calling `remove_on_set_parameters_callback` more than once with the same handler, + * or calling it after the shared pointer has been reset is an error. + * Resetting or letting the smart pointer go out of scope after calling + * `remove_on_set_parameters_callback` is not a problem. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_on_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); + + /// Register a callback to be called anytime a parameter is about to be changed. + /** + * With this method, only one callback can be set at a time. The callback that was previously + * set by this method is returned or `nullptr` if no callback was previously set. + * + * The callbacks added with `add_on_set_parameters_callback` are stored in a different place. + * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this + * method. For removing it, use `set_on_parameters_set_callback(nullptr)`. + * + * \param[in] callback The callback to be called when the value for a + * parameter is about to be set. + * \return The previous callback that was registered, if there was one, + * otherwise nullptr. + */ + RCLCPP_PUBLIC + OnParametersSetCallbackType + set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback); + + /// Get the fully-qualified names of all available nodes. + /** + * The fully-qualified name includes the local namespace and name of the node. + * \return A vector of fully-qualified names of nodes. + */ + RCLCPP_PUBLIC + std::vector + get_node_names() const; + + RCLCPP_PUBLIC + std::map> + get_topic_names_and_types() const; + + RCLCPP_PUBLIC + std::map> + get_service_names_and_types() const; + + RCLCPP_PUBLIC + size_t + count_publishers(const std::string & topic_name) const; + + RCLCPP_PUBLIC + size_t + count_subscribers(const std::string & topic_name) const; + + /// Return a graph event, which will be set anytime a graph change occurs. + /* The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the loan just let it go + * out of scope. + */ + RCLCPP_PUBLIC + rclcpp::Event::SharedPtr + get_graph_event(); + + /// Wait for a graph event to occur by waiting on an Event to become set. + /** + * The given Event must be acquire through the get_graph_event() method. + * + * \throws InvalidEventError if the given event is nullptr + * \throws EventNotRegisteredError if the given event was not acquired with + * get_graph_event(). + */ + RCLCPP_PUBLIC + void + wait_for_graph_change( + rclcpp::Event::SharedPtr event, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + rclcpp::Clock::SharedPtr + get_clock(); + + RCLCPP_PUBLIC + Time + now(); + + /// Return the Node's internal NodeBaseInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface(); + + /// Return the Node's internal NodeClockInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeClockInterface::SharedPtr + get_node_clock_interface(); + + /// Return the Node's internal NodeGraphInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr + get_node_graph_interface(); + + /// Return the Node's internal NodeLoggingInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface(); + + /// Return the Node's internal NodeTimersInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr + get_node_timers_interface(); + + /// Return the Node's internal NodeTopicsInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface(); + + /// Return the Node's internal NodeServicesInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface(); + + /// Return the Node's internal NodeWaitablesInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface(); + + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + get_node_parameters_interface(); + + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface(); + + /// Return the sub-namespace, if this is a sub-node, otherwise an empty string. + /** + * The returned sub-namespace is either the accumulated sub-namespaces which + * were given to one-to-many create_sub_node() calls, or an empty string if + * this is an original node instance, i.e. not a sub-node. + * + * For example, consider: + * + * ```cpp + * auto node = std::make_shared("my_node", "my_ns"); + * node->get_sub_namespace(); // -> "" + * auto sub_node1 = node->create_sub_node("a"); + * sub_node1->get_sub_namespace(); // -> "a" + * auto sub_node2 = sub_node1->create_sub_node("b"); + * sub_node2->get_sub_namespace(); // -> "a/b" + * auto sub_node3 = node->create_sub_node("foo"); + * sub_node3->get_sub_namespace(); // -> "foo" + * node->get_sub_namespace(); // -> "" + * ``` + * + * get_namespace() will return the original node namespace, and will not + * include the sub-namespace if one exists. + * To get that you need to call the get_effective_namespace() method. + * + * \sa get_namespace() + * \sa get_effective_namespace() + * \return the sub-namespace string, not including the node's original namespace + */ + RCLCPP_PUBLIC + const std::string & + get_sub_namespace() const; + + /// Return the effective namespace that is used when creating entities. + /** + * The returned namespace is a concatenation of the node namespace and the + * accumulated sub-namespaces, which is used as the namespace when creating + * entities which have relative names. + * + * For example, consider: + * + * ```cpp + * auto node = std::make_shared("my_node", "my_ns"); + * node->get_effective_namespace(); // -> "/my_ns" + * auto sub_node1 = node->create_sub_node("a"); + * sub_node1->get_effective_namespace(); // -> "/my_ns/a" + * auto sub_node2 = sub_node1->create_sub_node("b"); + * sub_node2->get_effective_namespace(); // -> "/my_ns/a/b" + * auto sub_node3 = node->create_sub_node("foo"); + * sub_node3->get_effective_namespace(); // -> "/my_ns/foo" + * node->get_effective_namespace(); // -> "/my_ns" + * ``` + * + * \sa get_namespace() + * \sa get_sub_namespace() + * \return the sub-namespace string, not including the node's original namespace + */ + RCLCPP_PUBLIC + const std::string & + get_effective_namespace() const; + + /// Create a sub-node, which will extend the namespace of all entities created with it. + /** + * A sub-node (short for subordinate node) is an instance of this class + * which has been created using an existing instance of this class, but which + * has an additional sub-namespace (short for subordinate namespace) + * associated with it. + * The sub-namespace will extend the node's namespace for the purpose of + * creating additional entities, such as Publishers, Subscriptions, Service + * Clients and Servers, and so on. + * + * By default, when an instance of this class is created using one of the + * public constructors, it has no sub-namespace associated with it, and + * therefore is not a sub-node. + * That "normal" node instance may, however, be used to create further + * instances of this class, based on the original instance, which have an + * additional sub-namespace associated with them. + * This may be done by using this method, create_sub_node(). + * + * Furthermore, a sub-node may be used to create additional sub-node's, in + * which case the sub-namespace passed to this function will further + * extend the sub-namespace of the existing sub-node. + * See get_sub_namespace() and get_effective_namespace() for examples. + * + * Note that entities which use absolute names are not affected by any + * namespaces, neither the normal node namespace nor any sub-namespace. + * Note also that the fully qualified node name is unaffected by a + * sub-namespace. + * + * The sub-namespace should be relative, and an exception will be thrown if + * the sub-namespace is absolute, i.e. if it starts with a leading '/'. + * + * \sa get_sub_namespace() + * \sa get_effective_namespace() + * \param[in] sub_namespace sub-namespace of the sub-node. + * \return newly created sub-node + * \throws NameValidationError if the sub-namespace is absolute, i.e. starts + * with a leading '/'. + */ + RCLCPP_PUBLIC + rclcpp::Node::SharedPtr + create_sub_node(const std::string & sub_namespace); + + /// Return the NodeOptions used when creating this node. + RCLCPP_PUBLIC + const rclcpp::NodeOptions & + get_node_options() const; + + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). + /** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the creator + * of this node may manually call `assert_liveliness` at some point in time to signal to the rest + * of the system that this Node is still alive. + * + * \return `true` if the liveliness was asserted successfully, otherwise `false` + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + bool + assert_liveliness() const; + +protected: + /// Construct a sub-node, which will extend the namespace of all entities created with it. + /** + * \sa create_sub_node() + * + * \param[in] other The node from which a new sub-node is created. + * \param[in] sub_namespace The sub-namespace of the sub-node. + */ + RCLCPP_PUBLIC + Node( + const Node & other, + const std::string & sub_namespace); + +private: + RCLCPP_DISABLE_COPY(Node) + + RCLCPP_PUBLIC + bool + group_in_node(callback_group::CallbackGroup::SharedPtr group); + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; + + const rclcpp::NodeOptions node_options_; + const std::string sub_namespace_; + const std::string effective_namespace_; +}; + +} // namespace rclcpp + +#ifndef RCLCPP__NODE_IMPL_HPP_ +// Template implementations +#include "node_impl.hpp" +#endif + +#endif // RCLCPP__NODE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_impl.hpp new file mode 100644 index 0000000..ab0c8f8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_impl.hpp @@ -0,0 +1,272 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_IMPL_HPP_ +#define RCLCPP__NODE_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/create_client.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_service.hpp" +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +#ifndef RCLCPP__NODE_HPP_ +#include "node.hpp" +#endif + +namespace rclcpp +{ + +RCLCPP_LOCAL +inline +std::string +extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace) +{ + std::string name_with_sub_namespace(name); + if (sub_namespace != "" && name.front() != '/' && name.front() != '~') { + name_with_sub_namespace = sub_namespace + "/" + name; + } + return name_with_sub_namespace; +} + +template +std::shared_ptr +Node::create_publisher( + const std::string & topic_name, + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options) +{ + return rclcpp::create_publisher( + *this, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, + options); +} + +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename CallbackMessageT, + typename SubscriptionT, + typename MessageMemoryStrategyT> +std::shared_ptr +Node::create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) +{ + return rclcpp::create_subscription( + *this, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, + std::forward(callback), + options, + msg_mem_strat); +} + +template +typename rclcpp::WallTimer::SharedPtr +Node::create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + auto timer = rclcpp::WallTimer::make_shared( + std::chrono::duration_cast(period), + std::move(callback), + this->node_base_->get_context()); + node_timers_->add_timer(timer, group); + return timer; +} + +template +typename Client::SharedPtr +Node::create_client( + const std::string & service_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_client( + node_base_, + node_graph_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + qos_profile, + group); +} + +template +typename rclcpp::Service::SharedPtr +Node::create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_service( + node_base_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + std::forward(callback), + qos_profile, + group); +} + +template +auto +Node::declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + return this->declare_parameter( + name, + rclcpp::ParameterValue(default_value), + parameter_descriptor, + ignore_override + ).get(); +} + +template +std::vector +Node::declare_parameters( + const std::string & namespace_, + const std::map & parameters, + bool ignore_overrides) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace, ignore_overrides](auto element) { + return this->declare_parameter( + normalized_namespace + element.first, + element.second, + rcl_interfaces::msg::ParameterDescriptor(), + ignore_overrides); + } + ); + return result; +} + +template +std::vector +Node::declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters, + bool ignore_overrides) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace, ignore_overrides](auto element) { + return static_cast( + this->declare_parameter( + normalized_namespace + element.first, + element.second.first, + element.second.second, + ignore_overrides) + ); + } + ); + return result; +} + +template +bool +Node::get_parameter(const std::string & name, ParameterT & parameter) const +{ + std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace()); + + rclcpp::Parameter parameter_variant; + + bool result = get_parameter(sub_name, parameter_variant); + if (result) { + parameter = static_cast(parameter_variant.get_value()); + } + + return result; +} + +template +bool +Node::get_parameter_or( + const std::string & name, + ParameterT & parameter, + const ParameterT & alternative_value) const +{ + std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace()); + + bool got_parameter = get_parameter(sub_name, parameter); + if (!got_parameter) { + parameter = alternative_value; + } + return got_parameter; +} + +// this is a partially-specialized version of get_parameter above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +bool +Node::get_parameters( + const std::string & prefix, + std::map & values) const +{ + std::map params; + bool result = node_parameters_->get_parameters_by_prefix(prefix, params); + if (result) { + for (const auto & param : params) { + values[param.first] = static_cast(param.second.get_value()); + } + } + + return result; +} + +} // namespace rclcpp + +#endif // RCLCPP__NODE_IMPL_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp new file mode 100644 index 0000000..cfc5917 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp @@ -0,0 +1,149 @@ +// Copyright 2019 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 RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +/// This header provides the get_node_base_interface() template function. +/** + * This function is useful for getting the NodeBaseInterface pointer from + * various kinds of Node-like classes. + * + * It's able to get the NodeBaseInterface pointer so long as the class + * has a method called ``get_node_base_interface()`` which returns + * either a pointer (const or not) to a NodeBaseInterface or a + * std::shared_ptr to a NodeBaseInterface. + */ + +namespace rclcpp +{ +namespace node_interfaces +{ + +namespace detail +{ + +// This is a meta-programming checker for if a given Node-like object has a +// getter called get_node_base_interface() which returns various types, +// e.g. const pointer or a shared pointer. +template +struct has_get_node_base_interface +{ +private: + template + static constexpr + auto + check(T *)->typename std::is_same< + decltype(std::declval().get_node_base_interface()), + ReturnType + >::type; + + template + static constexpr + std::false_type + check(...); + +public: + using type = decltype(check(nullptr)); + static constexpr bool value = type::value; +}; + +// If NodeType is a pointer to NodeBaseInterface already (just normal function overload). +inline +rclcpp::node_interfaces::NodeBaseInterface * +get_node_base_interface_from_pointer(rclcpp::node_interfaces::NodeBaseInterface * pointer) +{ + return pointer; +} + +// If NodeType has a method called get_node_base_interface() which returns a shared pointer. +template< + typename NodeType, + typename std::enable_if::type, + std::shared_ptr + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeBaseInterface * +get_node_base_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_base_interface().get(); +} + +// If NodeType has a method called get_node_base_interface() which returns a pointer. +template< + typename NodeType, + typename std::enable_if::type, + rclcpp::node_interfaces::NodeBaseInterface * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeBaseInterface * +get_node_base_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_base_interface(); +} + +// Forward shared_ptr's to const node pointer signatures. +template< + typename NodeType, + typename std::enable_if::type::element_type> * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeBaseInterface * +get_node_base_interface_from_pointer(NodeType node_shared_pointer) +{ + return get_node_base_interface_from_pointer(node_shared_pointer->get()); +} + +} // namespace detail + +/// Get the NodeBaseInterface as a pointer from a pointer to a "Node like" object. +template< + typename NodeType, + typename std::enable_if::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeBaseInterface * +get_node_base_interface(NodeType node_pointer) +{ + // Forward pointers to detail implmentation directly. + return detail::get_node_base_interface_from_pointer(node_pointer); +} + +/// Get the NodeBaseInterface as a pointer from a "Node like" object. +template< + typename NodeType, + typename std::enable_if< + !std::is_pointer::type>::value, int + >::type = 0 +> +rclcpp::node_interfaces::NodeBaseInterface * +get_node_base_interface(NodeType && node_reference) +{ + // Forward references to detail implmentation as a pointer. + return detail::get_node_base_interface_from_pointer(&node_reference); +} + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp new file mode 100644 index 0000000..9c54f06 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp @@ -0,0 +1,149 @@ +// Copyright 2019 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 RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_timers_interface.hpp" + +/// This header provides the get_node_timers_interface() template function. +/** + * This function is useful for getting the NodeTimersInterface pointer from + * various kinds of Node-like classes. + * + * It's able to get the NodeTimersInterface pointer so long as the class + * has a method called ``get_node_timers_interface()`` which returns + * either a pointer (const or not) to a NodeTimersInterface or a + * std::shared_ptr to a NodeTimersInterface. + */ + +namespace rclcpp +{ +namespace node_interfaces +{ + +namespace detail +{ + +// This is a meta-programming checker for if a given Node-like object has a +// getter called get_node_timers_interface() which returns various types, +// e.g. const pointer or a shared pointer. +template +struct has_get_node_timers_interface +{ +private: + template + static constexpr + auto + check(T *)->typename std::is_same< + decltype(std::declval().get_node_timers_interface()), + ReturnType + >::type; + + template + static constexpr + std::false_type + check(...); + +public: + using type = decltype(check(nullptr)); + static constexpr bool value = type::value; +}; + +// If NodeType is a pointer to NodeTimersInterface already (just normal function overload). +inline +rclcpp::node_interfaces::NodeTimersInterface * +get_node_timers_interface_from_pointer(rclcpp::node_interfaces::NodeTimersInterface * pointer) +{ + return pointer; +} + +// If NodeType has a method called get_node_timers_interface() which returns a shared pointer. +template< + typename NodeType, + typename std::enable_if::type, + std::shared_ptr + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTimersInterface * +get_node_timers_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_timers_interface().get(); +} + +// If NodeType has a method called get_node_timers_interface() which returns a pointer. +template< + typename NodeType, + typename std::enable_if::type, + rclcpp::node_interfaces::NodeTimersInterface * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTimersInterface * +get_node_timers_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_timers_interface(); +} + +// Forward shared_ptr's to const node pointer signatures. +template< + typename NodeType, + typename std::enable_if::type::element_type> * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTimersInterface * +get_node_timers_interface_from_pointer(NodeType node_shared_pointer) +{ + return get_node_timers_interface_from_pointer(node_shared_pointer->get()); +} + +} // namespace detail + +/// Get the NodeTimersInterface as a pointer from a pointer to a "Node like" object. +template< + typename NodeType, + typename std::enable_if::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTimersInterface * +get_node_timers_interface(NodeType node_pointer) +{ + // Forward pointers to detail implmentation directly. + return detail::get_node_timers_interface_from_pointer(node_pointer); +} + +/// Get the NodeTimersInterface as a pointer from a "Node like" object. +template< + typename NodeType, + typename std::enable_if< + !std::is_pointer::type>::value, int + >::type = 0 +> +rclcpp::node_interfaces::NodeTimersInterface * +get_node_timers_interface(NodeType && node_reference) +{ + // Forward references to detail implmentation as a pointer. + return detail::get_node_timers_interface_from_pointer(&node_reference); +} + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp new file mode 100644 index 0000000..c842342 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp @@ -0,0 +1,149 @@ +// Copyright 2019 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 RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_topics_interface.hpp" + +/// This header provides the get_node_topics_interface() template function. +/** + * This function is useful for getting the NodeTopicsInterface pointer from + * various kinds of Node-like classes. + * + * It's able to get the NodeTopicsInterface pointer so long as the class + * has a method called ``get_node_topics_interface()`` which returns + * either a pointer (const or not) to a NodeTopicsInterface or a + * std::shared_ptr to a NodeTopicsInterface. + */ + +namespace rclcpp +{ +namespace node_interfaces +{ + +namespace detail +{ + +// This is a meta-programming checker for if a given Node-like object has a +// getter called get_node_topics_interface() which returns various types, +// e.g. const pointer or a shared pointer. +template +struct has_get_node_topics_interface +{ +private: + template + static constexpr + auto + check(T *)->typename std::is_same< + decltype(std::declval().get_node_topics_interface()), + ReturnType + >::type; + + template + static constexpr + std::false_type + check(...); + +public: + using type = decltype(check(nullptr)); + static constexpr bool value = type::value; +}; + +// If NodeType is a pointer to NodeTopicsInterface already (just normal function overload). +inline +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(rclcpp::node_interfaces::NodeTopicsInterface * pointer) +{ + return pointer; +} + +// If NodeType has a method called get_node_topics_interface() which returns a shared pointer. +template< + typename NodeType, + typename std::enable_if::type, + std::shared_ptr + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_topics_interface().get(); +} + +// If NodeType has a method called get_node_topics_interface() which returns a pointer. +template< + typename NodeType, + typename std::enable_if::type, + rclcpp::node_interfaces::NodeTopicsInterface * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(NodeType node_pointer) +{ + return node_pointer->get_node_topics_interface(); +} + +// Forward shared_ptr's to const node pointer signatures. +template< + typename NodeType, + typename std::enable_if::type::element_type> * + >::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface_from_pointer(NodeType node_shared_pointer) +{ + return get_node_topics_interface_from_pointer(node_shared_pointer->get()); +} + +} // namespace detail + +/// Get the NodeTopicsInterface as a pointer from a pointer to a "Node like" object. +template< + typename NodeType, + typename std::enable_if::value, int>::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface(NodeType node_pointer) +{ + // Forward pointers to detail implmentation directly. + return detail::get_node_topics_interface_from_pointer(node_pointer); +} + +/// Get the NodeTopicsInterface as a pointer from a "Node like" object. +template< + typename NodeType, + typename std::enable_if< + !std::is_pointer::type>::value, int + >::type = 0 +> +rclcpp::node_interfaces::NodeTopicsInterface * +get_node_topics_interface(NodeType && node_reference) +{ + // Forward references to detail implmentation as a pointer. + return detail::get_node_topics_interface_from_pointer(&node_reference); +} + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__GET_NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base.hpp new file mode 100644 index 0000000..5bede31 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -0,0 +1,159 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ + +#include +#include +#include + +#include "rcl/node.h" +#include "rclcpp/context.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeBase part of the Node API. +class NodeBase : public NodeBaseInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase) + + RCLCPP_PUBLIC + NodeBase( + const std::string & node_name, + const std::string & namespace_, + rclcpp::Context::SharedPtr context, + const rcl_node_options_t & rcl_node_options, + bool use_intra_process_default); + + RCLCPP_PUBLIC + virtual + ~NodeBase(); + + RCLCPP_PUBLIC + virtual + const char * + get_name() const; + + RCLCPP_PUBLIC + virtual + const char * + get_namespace() const; + + RCLCPP_PUBLIC + virtual + const char * + get_fully_qualified_name() const; + + RCLCPP_PUBLIC + virtual + rclcpp::Context::SharedPtr + get_context(); + + RCLCPP_PUBLIC + virtual + rcl_node_t * + get_rcl_node_handle(); + + RCLCPP_PUBLIC + virtual + const rcl_node_t * + get_rcl_node_handle() const; + + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle(); + + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle() const; + + RCLCPP_PUBLIC + virtual + bool + assert_liveliness() const; + + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + get_default_callback_group(); + + RCLCPP_PUBLIC + virtual + bool + callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + + RCLCPP_PUBLIC + virtual + const std::vector & + get_callback_groups() const; + + RCLCPP_PUBLIC + virtual + std::atomic_bool & + get_associated_with_executor_atomic(); + + RCLCPP_PUBLIC + virtual + rcl_guard_condition_t * + get_notify_guard_condition(); + + RCLCPP_PUBLIC + virtual + std::unique_lock + acquire_notify_guard_condition_lock() const; + + RCLCPP_PUBLIC + virtual + bool + get_use_intra_process_default() const; + +private: + RCLCPP_DISABLE_COPY(NodeBase) + + rclcpp::Context::SharedPtr context_; + bool use_intra_process_default_; + + std::shared_ptr node_handle_; + + rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_; + std::vector callback_groups_; + + std::atomic_bool associated_with_executor_; + + /// Guard condition for notifying the Executor of changes to this node. + mutable std::recursive_mutex notify_guard_condition_mutex_; + rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + bool notify_guard_condition_is_valid_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp new file mode 100644 index 0000000..d0247ee --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -0,0 +1,169 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "rcl/node.h" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeBase part of the Node API. +class NodeBaseInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) + + RCLCPP_PUBLIC + virtual + ~NodeBaseInterface() = default; + + /// Return the name of the node. + /** \return The name of the node. */ + RCLCPP_PUBLIC + virtual + const char * + get_name() const = 0; + + /// Return the namespace of the node. + /** \return The namespace of the node. */ + RCLCPP_PUBLIC + virtual + const char * + get_namespace() const = 0; + + /// Return the fully qualified name of the node. + /** \return The fully qualified name of the node. */ + RCLCPP_PUBLIC + virtual + const char * + get_fully_qualified_name() const = 0; + + /// Return the context of the node. + /** \return SharedPtr to the node's context. */ + RCLCPP_PUBLIC + virtual + rclcpp::Context::SharedPtr + get_context() = 0; + + /// Return the rcl_node_t node handle (non-const version). + RCLCPP_PUBLIC + virtual + rcl_node_t * + get_rcl_node_handle() = 0; + + /// Return the rcl_node_t node handle (const version). + RCLCPP_PUBLIC + virtual + const rcl_node_t * + get_rcl_node_handle() const = 0; + + /// Return the rcl_node_t node handle in a std::shared_ptr. + /** + * This handle remains valid after the Node is destroyed. + * The actual rcl node is not finalized until it is out of scope everywhere. + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle() = 0; + + /// Return the rcl_node_t node handle in a std::shared_ptr. + /** + * This handle remains valid after the Node is destroyed. + * The actual rcl node is not finalized until it is out of scope everywhere. + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle() const = 0; + + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE). + RCLCPP_PUBLIC + virtual + bool + assert_liveliness() const = 0; + + /// Create and return a callback group. + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0; + + /// Return the default callback group. + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + get_default_callback_group() = 0; + + /// Return true if the given callback group is associated with this node. + RCLCPP_PUBLIC + virtual + bool + callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + + /// Return list of callback groups associated with this node. + RCLCPP_PUBLIC + virtual + const std::vector & + get_callback_groups() const = 0; + + /// Return the atomic bool which is used to ensure only one executor is used. + RCLCPP_PUBLIC + virtual + std::atomic_bool & + get_associated_with_executor_atomic() = 0; + + /// Return guard condition that should be notified when the internal node state changes. + /** + * For example, this should be notified when a publisher is added or removed. + * + * \return the rcl_guard_condition_t if it is valid, else nullptr + */ + RCLCPP_PUBLIC + virtual + rcl_guard_condition_t * + get_notify_guard_condition() = 0; + + /// Acquire and return a scoped lock that protects the notify guard condition. + /** This should be used when triggering the notify guard condition. */ + RCLCPP_PUBLIC + virtual + std::unique_lock + acquire_notify_guard_condition_lock() const = 0; + + /// Return the default preference for using intra process communication. + RCLCPP_PUBLIC + virtual + bool + get_use_intra_process_default() const = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp new file mode 100644 index 0000000..e0e7059 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -0,0 +1,72 @@ +// Copyright 2017 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 RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/time_source.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeClock part of the Node API. +class NodeClock : public NodeClockInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClock) + + RCLCPP_PUBLIC + explicit NodeClock( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging); + + RCLCPP_PUBLIC + virtual + ~NodeClock(); + + /// Get a clock which will be kept up to date by the node. + RCLCPP_PUBLIC + virtual + rclcpp::Clock::SharedPtr + get_clock(); + +private: + RCLCPP_DISABLE_COPY(NodeClock) + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + + rclcpp::Clock::SharedPtr ros_clock_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp new file mode 100644 index 0000000..055d475 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -0,0 +1,48 @@ +// Copyright 2017 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 RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeClock part of the Node API. +class NodeClockInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface) + + RCLCPP_PUBLIC + virtual + ~NodeClockInterface() = default; + + /// Get a ROS clock which will be kept up to date by the node. + RCLCPP_PUBLIC + virtual + rclcpp::Clock::SharedPtr + get_clock() = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp new file mode 100644 index 0000000..8ecaf07 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -0,0 +1,145 @@ +// Copyright 2016-2017 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 RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/guard_condition.h" + +#include "rclcpp/event.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +namespace graph_listener +{ +class GraphListener; +} // namespace graph_listener + +namespace node_interfaces +{ + +/// Implementation the NodeGraph part of the Node API. +class NodeGraph : public NodeGraphInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraph) + + RCLCPP_PUBLIC + explicit NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeGraph(); + + RCLCPP_PUBLIC + virtual + std::map> + get_topic_names_and_types(bool no_demangle = false) const; + + RCLCPP_PUBLIC + virtual + std::map> + get_service_names_and_types() const; + + RCLCPP_PUBLIC + virtual + std::vector + get_node_names() const; + + RCLCPP_PUBLIC + virtual + std::vector> + get_node_names_and_namespaces() const; + + RCLCPP_PUBLIC + virtual + size_t + count_publishers(const std::string & topic_name) const; + + RCLCPP_PUBLIC + virtual + size_t + count_subscribers(const std::string & topic_name) const; + + RCLCPP_PUBLIC + virtual + const rcl_guard_condition_t * + get_graph_guard_condition() const; + + RCLCPP_PUBLIC + virtual + void + notify_graph_change(); + + RCLCPP_PUBLIC + virtual + void + notify_shutdown(); + + RCLCPP_PUBLIC + virtual + rclcpp::Event::SharedPtr + get_graph_event(); + + RCLCPP_PUBLIC + virtual + void + wait_for_graph_change( + rclcpp::Event::SharedPtr event, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + virtual + size_t + count_graph_users(); + +private: + RCLCPP_DISABLE_COPY(NodeGraph) + + /// Handle to the NodeBaseInterface given in the constructor. + rclcpp::node_interfaces::NodeBaseInterface * node_base_; + + /// Graph Listener which waits on graph changes for the node and is shared across nodes. + std::shared_ptr graph_listener_; + /// Whether or not this node needs to be added to the graph listener. + std::atomic_bool should_add_to_graph_listener_; + + /// Mutex to guard the graph event related data structures. + mutable std::mutex graph_mutex_; + /// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()). + std::condition_variable graph_cv_; + /// Weak references to graph events out on loan. + std::vector graph_events_; + /// Number of graph events out on loan, used to determine if the graph should be monitored. + /** graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */ + std::atomic_size_t graph_users_count_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp new file mode 100644 index 0000000..022984d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -0,0 +1,158 @@ +// Copyright 2016-2017 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 RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl/guard_condition.h" + +#include "rclcpp/event.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeGraph part of the Node API. +class NodeGraphInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface) + + RCLCPP_PUBLIC + virtual + ~NodeGraphInterface() = default; + + /// Return a map of existing topic names to list of topic types. + /** + * A topic is considered to exist when at least one publisher or subscriber + * exists for it, whether they be local or remote to this process. + * + * \param[in] no_demangle if true, topic names and types are not demangled + */ + RCLCPP_PUBLIC + virtual + std::map> + get_topic_names_and_types(bool no_demangle = false) const = 0; + + /// Return a map of existing service names to list of service types. + /** + * A service is considered to exist when at least one service server or + * service client exists for it, whether they be local or remote to this + * process. + */ + RCLCPP_PUBLIC + virtual + std::map> + get_service_names_and_types() const = 0; + + /// Return a vector of existing node names (string). + RCLCPP_PUBLIC + virtual + std::vector + get_node_names() const = 0; + + /// Return a vector of existing node names and namespaces (pair of string). + RCLCPP_PUBLIC + virtual + std::vector> + get_node_names_and_namespaces() const = 0; + + /// Return the number of publishers that are advertised on a given topic. + RCLCPP_PUBLIC + virtual + size_t + count_publishers(const std::string & topic_name) const = 0; + + /// Return the number of subscribers who have created a subscription for a given topic. + RCLCPP_PUBLIC + virtual + size_t + count_subscribers(const std::string & topic_name) const = 0; + + /// Return the rcl guard condition which is triggered when the ROS graph changes. + RCLCPP_PUBLIC + virtual + const rcl_guard_condition_t * + get_graph_guard_condition() const = 0; + + /// Notify threads waiting on graph changes. + /** + * Affects threads waiting on the notify guard condition, see: + * get_notify_guard_condition(), as well as the threads waiting on graph + * changes using a graph Event, see: wait_for_graph_change(). + * + * This is typically only used by the rclcpp::graph_listener::GraphListener. + * + * \throws RCLBaseError (a child of that exception) when an rcl error occurs + */ + RCLCPP_PUBLIC + virtual + void + notify_graph_change() = 0; + + /// Notify any and all blocking node actions that shutdown has occurred. + RCLCPP_PUBLIC + virtual + void + notify_shutdown() = 0; + + /// Return a graph event, which will be set anytime a graph change occurs. + /** + * The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the load just let it go + * out of scope. + */ + RCLCPP_PUBLIC + virtual + rclcpp::Event::SharedPtr + get_graph_event() = 0; + + /// Wait for a graph event to occur by waiting on an Event to become set. + /** + * The given Event must be acquire through the get_graph_event() method. + * + * \throws InvalidEventError if the given event is nullptr + * \throws EventNotRegisteredError if the given event was not acquired with + * get_graph_event(). + */ + RCLCPP_PUBLIC + virtual + void + wait_for_graph_change( + rclcpp::Event::SharedPtr event, + std::chrono::nanoseconds timeout) = 0; + + /// Return the number of on loan graph events, see get_graph_event(). + /** + * This is typically only used by the rclcpp::graph_listener::GraphListener. + */ + RCLCPP_PUBLIC + virtual + size_t + count_graph_users() = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp new file mode 100644 index 0000000..9694bf3 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp @@ -0,0 +1,66 @@ +// Copyright 2017 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 RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_ + +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeLogging part of the Node API. +class NodeLogging : public NodeLoggingInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) + + RCLCPP_PUBLIC + explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeLogging(); + + RCLCPP_PUBLIC + virtual + rclcpp::Logger + get_logger() const; + + RCLCPP_PUBLIC + virtual + const char * + get_logger_name() const; + +private: + RCLCPP_DISABLE_COPY(NodeLogging) + + /// Handle to the NodeBaseInterface given in the constructor. + rclcpp::node_interfaces::NodeBaseInterface * node_base_; + + rclcpp::Logger logger_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp new file mode 100644 index 0000000..ace0afc --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -0,0 +1,58 @@ +// Copyright 2017 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 RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_ + +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeLogging part of the Node API. +class NodeLoggingInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) + + RCLCPP_PUBLIC + virtual + ~NodeLoggingInterface() = default; + + /// Return the logger of the node. + /** \return The logger of the node. */ + RCLCPP_PUBLIC + virtual + rclcpp::Logger + get_logger() const = 0; + + /// Return the logger name associated with the node. + /** \return The logger name associated with the node. */ + RCLCPP_PUBLIC + virtual + const char * + get_logger_name() const = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp new file mode 100644 index 0000000..6888cdf --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -0,0 +1,215 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ + +#include +#include +#include +#include +#include + +#include "rcutils/macros.h" + +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_service.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +// Internal struct for holding useful info about parameters +struct ParameterInfo +{ + /// Current value of the parameter. + rclcpp::ParameterValue value; + + /// A description of the parameter + rcl_interfaces::msg::ParameterDescriptor descriptor; +}; + +// Internal RAII-style guard for mutation recursion +class ParameterMutationRecursionGuard +{ +public: + explicit ParameterMutationRecursionGuard(bool & allow_mod) + : allow_modification_(allow_mod) + { + if (!allow_modification_) { + throw rclcpp::exceptions::ParameterModifiedInCallbackException( + "cannot set or declare a parameter, or change the callback from within set callback"); + } + + allow_modification_ = false; + } + + ~ParameterMutationRecursionGuard() + { + allow_modification_ = true; + } + +private: + bool & allow_modification_; +}; + +/// Implementation of the NodeParameters part of the Node API. +class NodeParameters : public NodeParametersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters) + + RCLCPP_PUBLIC + NodeParameters( + const node_interfaces::NodeBaseInterface::SharedPtr node_base, + const node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + const node_interfaces::NodeServicesInterface::SharedPtr node_services, + const node_interfaces::NodeClockInterface::SharedPtr node_clock, + const std::vector & parameter_overrides, + bool start_parameter_services, + bool start_parameter_event_publisher, + const rclcpp::QoS & parameter_event_qos, + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, + bool allow_undeclared_parameters, + bool automatically_declare_parameters_from_overrides); + + RCLCPP_PUBLIC + virtual + ~NodeParameters(); + + RCLCPP_PUBLIC + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) override; + + RCLCPP_PUBLIC + void + undeclare_parameter(const std::string & name) override; + + RCLCPP_PUBLIC + bool + has_parameter(const std::string & name) const override; + + RCLCPP_PUBLIC + std::vector + set_parameters( + const std::vector & parameters) override; + + RCLCPP_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically( + const std::vector & parameters) override; + + RCLCPP_PUBLIC + std::vector + get_parameters(const std::vector & names) const override; + + RCLCPP_PUBLIC + rclcpp::Parameter + get_parameter(const std::string & name) const override; + + RCLCPP_PUBLIC + bool + get_parameter( + const std::string & name, + rclcpp::Parameter & parameter) const override; + + RCLCPP_PUBLIC + bool + get_parameters_by_prefix( + const std::string & prefix, + std::map & parameters) const override; + + RCLCPP_PUBLIC + std::vector + describe_parameters(const std::vector & names) const override; + + RCLCPP_PUBLIC + std::vector + get_parameter_types(const std::vector & names) const override; + + RCLCPP_PUBLIC + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const override; + + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnParametersSetCallbackType callback) override; + + RCLCPP_PUBLIC + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; + + RCLCPP_PUBLIC + OnParametersSetCallbackType + set_on_parameters_set_callback(OnParametersSetCallbackType callback) override; + + RCLCPP_PUBLIC + const std::map & + get_parameter_overrides() const override; + + using CallbacksContainerType = std::list; + +private: + RCLCPP_DISABLE_COPY(NodeParameters) + + mutable std::recursive_mutex mutex_; + + // There are times when we don't want to allow modifications to parameters + // (particularly when a set_parameter callback tries to call set_parameter, + // declare_parameter, etc). In those cases, this will be set to false. + bool parameter_modification_enabled_{true}; + + OnParametersSetCallbackType on_parameters_set_callback_ = nullptr; + + CallbacksContainerType on_parameters_set_callback_container_; + + std::map parameters_; + + std::map parameter_overrides_; + + bool allow_undeclared_ = false; + + Publisher::SharedPtr events_publisher_; + + std::shared_ptr parameter_service_; + + std::string combined_name_; + + node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + node_interfaces::NodeClockInterface::SharedPtr node_clock_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp new file mode 100644 index 0000000..f1102c0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -0,0 +1,213 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +struct OnSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) + + using OnParametersSetCallbackType = + std::function< + rcl_interfaces::msg::SetParametersResult( + const std::vector &)>; + + OnParametersSetCallbackType callback; +}; + +/// Pure virtual interface class for the NodeParameters part of the Node API. +class NodeParametersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface) + + RCLCPP_PUBLIC + virtual + ~NodeParametersInterface() = default; + + /// Declare and initialize a parameter. + /** + * \sa rclcpp::Node::declare_parameter + */ + RCLCPP_PUBLIC + virtual + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) = 0; + + /// Undeclare a parameter. + /** + * \sa rclcpp::Node::undeclare_parameter + */ + RCLCPP_PUBLIC + virtual + void + undeclare_parameter(const std::string & name) = 0; + + /// Return true if the parameter has been declared, otherwise false. + /** + * \sa rclcpp::Node::has_parameter + */ + RCLCPP_PUBLIC + virtual + bool + has_parameter(const std::string & name) const = 0; + + /// Set one or more parameters, one at a time. + /** + * \sa rclcpp::Node::set_parameters + */ + RCLCPP_PUBLIC + virtual + std::vector + set_parameters(const std::vector & parameters) = 0; + + /// Set and initialize a parameter, all at once. + /** + * \sa rclcpp::Node::set_parameters_atomically + */ + RCLCPP_PUBLIC + virtual + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically( + const std::vector & parameters) = 0; + + /// Get descriptions of parameters given their names. + /* + * \param[in] names a list of parameter names to check. + * \return the list of parameters that were found. + * Any parameter not found is omitted from the returned list. + */ + RCLCPP_PUBLIC + virtual + std::vector + get_parameters(const std::vector & names) const = 0; + + /// Get the description of one parameter given a name. + /* + * \param[in] name the name of the parameter to look for. + * \return the parameter if it exists on the node. + * \throws std::out_of_range if the parameter does not exist on the node. + */ + RCLCPP_PUBLIC + virtual + rclcpp::Parameter + get_parameter(const std::string & name) const = 0; + + /// Get the description of one parameter given a name. + /* + * \param[in] name the name of the parameter to look for. + * \param[out] parameter the description if parameter exists on the node. + * \return true if the parameter exists on the node, or + * \return false if the parameter does not exist. + */ + RCLCPP_PUBLIC + virtual + bool + get_parameter( + const std::string & name, + rclcpp::Parameter & parameter) const = 0; + + /// Get all parameters that have the specified prefix into the parameters map. + /* + * \param[in] prefix the name of the prefix to look for. + * \param[out] parameters a map of parameters that matched the prefix. + * \return true if any parameters with the prefix exists on the node, or + * \return false otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + get_parameters_by_prefix( + const std::string & prefix, + std::map & parameters) const = 0; + + RCLCPP_PUBLIC + virtual + std::vector + describe_parameters(const std::vector & names) const = 0; + + RCLCPP_PUBLIC + virtual + std::vector + get_parameter_types(const std::vector & names) const = 0; + + RCLCPP_PUBLIC + virtual + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; + + using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType; + + /// Add a callback for when parameters are being set. + /** + * \sa rclcpp::Node::add_on_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0; + + /// Remove a callback registered with `add_on_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_on_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0; + + /// Register a callback for when parameters are being set, return an existing one. + /** + * \sa rclcpp::Node::set_on_parameters_set_callback + */ + RCLCPP_PUBLIC + virtual + OnParametersSetCallbackType + set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0; + + /// Return the initial parameter values used by the NodeParameters to override default values. + RCLCPP_PUBLIC + virtual + const std::map & + get_parameter_overrides() const = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services.hpp new file mode 100644 index 0000000..0b0d99d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services.hpp @@ -0,0 +1,67 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeServices part of the Node API. +class NodeServices : public NodeServicesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServices) + + RCLCPP_PUBLIC + explicit NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeServices(); + + RCLCPP_PUBLIC + virtual + void + add_client( + rclcpp::ClientBase::SharedPtr client_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group); + + RCLCPP_PUBLIC + virtual + void + add_service( + rclcpp::ServiceBase::SharedPtr service_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group); + +private: + RCLCPP_DISABLE_COPY(NodeServices) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp new file mode 100644 index 0000000..c75fec8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -0,0 +1,57 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeServices part of the Node API. +class NodeServicesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface) + + RCLCPP_PUBLIC + virtual + ~NodeServicesInterface() = default; + + RCLCPP_PUBLIC + virtual + void + add_client( + rclcpp::ClientBase::SharedPtr client_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + + RCLCPP_PUBLIC + virtual + void + add_service( + rclcpp::ServiceBase::SharedPtr service_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp new file mode 100644 index 0000000..6ba37b7 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -0,0 +1,72 @@ +// Copyright 2018 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 RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/time_source.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTimeSource part of the Node API. +class NodeTimeSource : public NodeTimeSourceInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSource) + + RCLCPP_PUBLIC + explicit NodeTimeSource( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters + ); + + RCLCPP_PUBLIC + virtual + ~NodeTimeSource(); + +private: + RCLCPP_DISABLE_COPY(NodeTimeSource) + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + + rclcpp::TimeSource time_source_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp new file mode 100644 index 0000000..0f2df14 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -0,0 +1,42 @@ +// Copyright 2018 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 RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTimeSource part of the Node API. +class NodeTimeSourceInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTimeSourceInterface() = default; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp new file mode 100644 index 0000000..955e39c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp @@ -0,0 +1,60 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTimers part of the Node API. +class NodeTimers : public NodeTimersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimers) + + RCLCPP_PUBLIC + explicit NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeTimers(); + + /// Add a timer to the node. + RCLCPP_PUBLIC + virtual + void + add_timer( + rclcpp::TimerBase::SharedPtr timer, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); + +private: + RCLCPP_DISABLE_COPY(NodeTimers) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp new file mode 100644 index 0000000..422409a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -0,0 +1,50 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTimers part of the Node API. +class NodeTimersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTimersInterface() = default; + + /// Add a timer to the node. + RCLCPP_PUBLIC + virtual + void + add_timer( + rclcpp::TimerBase::SharedPtr timer, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp new file mode 100644 index 0000000..8209984 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -0,0 +1,86 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_ + +#include + +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTopics part of the Node API. +class NodeTopics : public NodeTopicsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) + + RCLCPP_PUBLIC + explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + ~NodeTopics() override; + + RCLCPP_PUBLIC + rclcpp::PublisherBase::SharedPtr + create_publisher( + const std::string & topic_name, + const rclcpp::PublisherFactory & publisher_factory, + const rclcpp::QoS & qos) override; + + RCLCPP_PUBLIC + void + add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; + + RCLCPP_PUBLIC + rclcpp::SubscriptionBase::SharedPtr + create_subscription( + const std::string & topic_name, + const rclcpp::SubscriptionFactory & subscription_factory, + const rclcpp::QoS & qos) override; + + RCLCPP_PUBLIC + void + add_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; + + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface * + get_node_base_interface() const override; + +private: + RCLCPP_DISABLE_COPY(NodeTopics) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp new file mode 100644 index 0000000..2beb7f0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -0,0 +1,86 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ + +#include +#include +#include + +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTopics part of the Node API. +class NodeTopicsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTopicsInterface() = default; + + RCLCPP_PUBLIC + virtual + rclcpp::PublisherBase::SharedPtr + create_publisher( + const std::string & topic_name, + const rclcpp::PublisherFactory & publisher_factory, + const rclcpp::QoS & qos) = 0; + + RCLCPP_PUBLIC + virtual + void + add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::SubscriptionBase::SharedPtr + create_subscription( + const std::string & topic_name, + const rclcpp::SubscriptionFactory & subscription_factory, + const rclcpp::QoS & qos) = 0; + + RCLCPP_PUBLIC + virtual + void + add_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::node_interfaces::NodeBaseInterface * + get_node_base_interface() const = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp new file mode 100644 index 0000000..e16ff8e --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp @@ -0,0 +1,67 @@ +// Copyright 2018 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 RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/waitable.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeWaitables part of the Node API. +class NodeWaitables : public NodeWaitablesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables) + + RCLCPP_PUBLIC + explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeWaitables(); + + RCLCPP_PUBLIC + virtual + void + add_waitable( + rclcpp::Waitable::SharedPtr waitable_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group); + + RCLCPP_PUBLIC + virtual + void + remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept; + +private: + RCLCPP_DISABLE_COPY(NodeWaitables) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp new file mode 100644 index 0000000..e848cea --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -0,0 +1,57 @@ +// Copyright 2018 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 RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeWaitables part of the Node API. +class NodeWaitablesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface) + + RCLCPP_PUBLIC + virtual + ~NodeWaitablesInterface() = default; + + RCLCPP_PUBLIC + virtual + void + add_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + + /// \note this function should not throw because it may be called in destructors + RCLCPP_PUBLIC + virtual + void + remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/node_options.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/node_options.hpp new file mode 100644 index 0000000..43141b6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/node_options.hpp @@ -0,0 +1,336 @@ +// Copyright 2019 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 RCLCPP__NODE_OPTIONS_HPP_ +#define RCLCPP__NODE_OPTIONS_HPP_ + +#include +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Encapsulation of options for node initialization. +class NodeOptions +{ +public: + /// Create NodeOptions with default values, optionally specifying the allocator to use. + /** + * Default values for the node options: + * + * - context = rclcpp::contexts::default_context::get_global_default_context() + * - arguments = {} + * - parameter_overrides = {} + * - use_global_arguments = true + * - use_intra_process_comms = false + * - start_parameter_services = true + * - start_parameter_event_publisher = true + * - parameter_event_qos = rclcpp::ParameterEventQoS + * - with history setting and depth from rmw_qos_profile_parameter_events + * - parameter_event_publisher_options = rclcpp::PublisherOptionsBase + * - allow_undeclared_parameters = false + * - automatically_declare_parameters_from_overrides = false + * - allocator = rcl_get_default_allocator() + * + * \param[in] allocator allocator to use in construction of NodeOptions. + */ + RCLCPP_PUBLIC + explicit NodeOptions(rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Destructor. + RCLCPP_PUBLIC + virtual + ~NodeOptions() = default; + + /// Copy constructor. + RCLCPP_PUBLIC + NodeOptions(const NodeOptions & other); + + /// Assignment operator. + RCLCPP_PUBLIC + NodeOptions & + operator=(const NodeOptions & other); + + /// Return the rcl_node_options used by the node. + /** + * This data structure is created lazily, on the first call to this function. + * Repeated calls will not regenerate it unless one of the input settings + * changed, like arguments, use_global_arguments, or the rcl allocator. + */ + RCLCPP_PUBLIC + const rcl_node_options_t * + get_rcl_node_options() const; + + /// Return the context to be used by the node. + RCLCPP_PUBLIC + rclcpp::Context::SharedPtr + context() const; + + /// Set the context, return this for parameter idiom. + RCLCPP_PUBLIC + NodeOptions & + context(rclcpp::Context::SharedPtr context); + + /// Return a reference to the list of arguments for the node. + RCLCPP_PUBLIC + const std::vector & + arguments() const; + + /// Set the arguments, return this for parameter idiom. + /** + * These arguments are used to extract remappings used by the node and other + * ROS specific settings, as well as user defined non-ROS arguments. + * + * This will cause the internal rcl_node_options_t struct to be invalidated. + */ + RCLCPP_PUBLIC + NodeOptions & + arguments(const std::vector & arguments); + + /// Return a reference to the list of parameter overrides. + RCLCPP_PUBLIC + std::vector & + parameter_overrides(); + + RCLCPP_PUBLIC + const std::vector & + parameter_overrides() const; + + /// Set the parameters overrides, return this for parameter idiom. + /** + * These parameter overrides are used to change the initial value + * of declared parameters within the node, overriding hard coded default + * values if necessary. + */ + RCLCPP_PUBLIC + NodeOptions & + parameter_overrides(const std::vector & parameter_overrides); + + /// Append a single parameter override, parameter idiom style. + template + NodeOptions & + append_parameter_override(const std::string & name, const ParameterT & value) + { + this->parameter_overrides().emplace_back(name, rclcpp::ParameterValue(value)); + return *this; + } + + /// Return the use_global_arguments flag. + RCLCPP_PUBLIC + bool + use_global_arguments() const; + + /// Set the use_global_arguments flag, return this for parameter idiom. + /** + * If true this will cause the node's behavior to be influenced by "global" + * arguments, i.e. arguments not targeted at specific nodes, as well as the + * arguments targeted at the current node. + * + * This will cause the internal rcl_node_options_t struct to be invalidated. + */ + RCLCPP_PUBLIC + NodeOptions & + use_global_arguments(bool use_global_arguments); + + /// Return the use_intra_process_comms flag. + RCLCPP_PUBLIC + bool + use_intra_process_comms() const; + + /// Set the use_intra_process_comms flag, return this for parameter idiom. + /** + * If true, messages on topics which are published and subscribed to within + * this context will go through a special intra-process communication code + * code path which can avoid serialization and deserialization, unnecessary + * copies, and achieve lower latencies in some cases. + * + * Defaults to false for now, as there are still some cases where it is not + * desirable. + */ + RCLCPP_PUBLIC + NodeOptions & + use_intra_process_comms(bool use_intra_process_comms); + + /// Return the start_parameter_services flag. + RCLCPP_PUBLIC + bool + start_parameter_services() const; + + /// Set the start_parameter_services flag, return this for parameter idiom. + /** + * If true, ROS services are created to allow external nodes to list, get, + * and request to set parameters of this node. + * + * If false, parameters will still work locally, but will not be accessible + * remotely. + * + * \sa start_parameter_event_publisher() + */ + RCLCPP_PUBLIC + NodeOptions & + start_parameter_services(bool start_parameter_services); + + /// Return the start_parameter_event_publisher flag. + RCLCPP_PUBLIC + bool + start_parameter_event_publisher() const; + + /// Set the start_parameter_event_publisher flag, return this for parameter idiom. + /** + * If true, a publisher is created on which an event message is published + * each time a parameter's state changes. + * This is used for recording and introspection, but is configurable + * separately from the other parameter services. + */ + RCLCPP_PUBLIC + NodeOptions & + start_parameter_event_publisher(bool start_parameter_event_publisher); + + /// Return a reference to the parameter_event_qos QoS. + RCLCPP_PUBLIC + const rclcpp::QoS & + parameter_event_qos() const; + + /// Set the parameter_event_qos QoS, return this for parameter idiom. + /** + * The QoS settings to be used for the parameter event publisher, if enabled. + */ + RCLCPP_PUBLIC + NodeOptions & + parameter_event_qos(const rclcpp::QoS & parameter_event_qos); + + /// Return a reference to the parameter_event_publisher_options. + RCLCPP_PUBLIC + const rclcpp::PublisherOptionsBase & + parameter_event_publisher_options() const; + + /// Set the parameter_event_publisher_options, return this for parameter idiom. + /** + * The QoS settings to be used for the parameter event publisher, if enabled. + * + * \todo(wjwwood): make this take/store an instance of + * rclcpp::PublisherOptionsWithAllocator, but to do that requires + * NodeOptions to also be templated based on the Allocator type. + */ + RCLCPP_PUBLIC + NodeOptions & + parameter_event_publisher_options( + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options); + + /// Return the allow_undeclared_parameters flag. + RCLCPP_PUBLIC + bool + allow_undeclared_parameters() const; + + /// Set the allow_undeclared_parameters, return this for parameter idiom. + /** + * If true, allow any parameter name to be set on the node without first + * being declared. + * Otherwise, setting an undeclared parameter will raise an exception. + * + * This option being true does not affect parameter_overrides, as the first + * set action will implicitly declare the parameter and therefore consider + * any parameter overrides. + */ + RCLCPP_PUBLIC + NodeOptions & + allow_undeclared_parameters(bool allow_undeclared_parameters); + + /// Return the automatically_declare_parameters_from_overrides flag. + RCLCPP_PUBLIC + bool + automatically_declare_parameters_from_overrides() const; + + /// Set the automatically_declare_parameters_from_overrides, return this. + /** + * If true, automatically iterate through the node's parameter overrides and + * implicitly declare any that have not already been declared. + * Otherwise, parameters passed to the node's parameter_overrides, and/or the + * global arguments (e.g. parameter overrides from a YAML file), which are + * not explicitly declared will not appear on the node at all, even if + * `allow_undeclared_parameters` is true. + * Already declared parameters will not be re-declared, and parameters + * declared in this way will use the default constructed ParameterDescriptor. + */ + RCLCPP_PUBLIC + NodeOptions & + automatically_declare_parameters_from_overrides( + bool automatically_declare_parameters_from_overrides); + + /// Return the rcl_allocator_t to be used. + RCLCPP_PUBLIC + const rcl_allocator_t & + allocator() const; + + /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_node_options_t. + /** + * This will cause the internal rcl_node_options_t struct to be invalidated. + */ + RCLCPP_PUBLIC + NodeOptions & + allocator(rcl_allocator_t allocator); + +protected: + /// Retrieve the ROS_DOMAIN_ID environment variable and populate options. + size_t + get_domain_id_from_env() const; + +private: + // This is mutable to allow for a const accessor which lazily creates the node options instance. + /// Underlying rcl_node_options structure. + mutable std::unique_ptr node_options_; + + // IMPORTANT: if any of these default values are changed, please update the + // documentation in this class. + + rclcpp::Context::SharedPtr context_ { + rclcpp::contexts::default_context::get_global_default_context()}; + + std::vector arguments_ {}; + + std::vector parameter_overrides_ {}; + + bool use_global_arguments_ {true}; + + bool use_intra_process_comms_ {false}; + + bool start_parameter_services_ {true}; + + bool start_parameter_event_publisher_ {true}; + + rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS( + rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) + ); + + rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase(); + + bool allow_undeclared_parameters_ {false}; + + bool automatically_declare_parameters_from_overrides_ {false}; + + rcl_allocator_t allocator_ {rcl_get_default_allocator()}; +}; + +} // namespace rclcpp + +#endif // RCLCPP__NODE_OPTIONS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/parameter.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter.hpp new file mode 100644 index 0000000..cee5585 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter.hpp @@ -0,0 +1,245 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__PARAMETER_HPP_ +#define RCLCPP__PARAMETER_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +class Parameter; + +namespace node_interfaces +{ +struct ParameterInfo; +} // namespace node_interfaces + +namespace detail +{ + +// This helper function is required because you cannot do specialization on a +// class method, so instead we specialize this template function and call it +// from the unspecialized, but dependent, class method. +template +auto +get_value_helper(const rclcpp::Parameter * parameter); + +} // namespace detail + +/// Structure to store an arbitrary parameter with templated get/set methods. +class Parameter +{ +public: + /// Construct with an empty name and a parameter value of type rclcpp::PARAMETER_NOT_SET. + RCLCPP_PUBLIC + Parameter(); + + /// Construct with given name and a parameter value of type rclcpp::PARAMETER_NOT_SET. + RCLCPP_PUBLIC + explicit Parameter(const std::string & name); + + /// Construct with given name and given parameter value. + RCLCPP_PUBLIC + Parameter(const std::string & name, const ParameterValue & value); + + /// Construct with given name and given parameter value. + template + Parameter(const std::string & name, ValueTypeT value) + : Parameter(name, ParameterValue(value)) + {} + + RCLCPP_PUBLIC + explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info); + + /// Equal operator. + RCLCPP_PUBLIC + bool + operator==(const Parameter & rhs) const; + + /// Not equal operator. + RCLCPP_PUBLIC + bool + operator!=(const Parameter & rhs) const; + + RCLCPP_PUBLIC + ParameterType + get_type() const; + + RCLCPP_PUBLIC + std::string + get_type_name() const; + + RCLCPP_PUBLIC + const std::string & + get_name() const; + + RCLCPP_PUBLIC + rcl_interfaces::msg::ParameterValue + get_value_message() const; + + /// Get the internal storage for the parameter value. + RCLCPP_PUBLIC + const rclcpp::ParameterValue & + get_parameter_value() const; + + /// Get value of parameter using rclcpp::ParameterType as template argument. + template + decltype(auto) + get_value() const + { + return value_.get(); + } + + /// Get value of parameter using c++ types as template argument. + template + decltype(auto) + get_value() const; + + RCLCPP_PUBLIC + bool + as_bool() const; + + RCLCPP_PUBLIC + int64_t + as_int() const; + + RCLCPP_PUBLIC + double + as_double() const; + + RCLCPP_PUBLIC + const std::string & + as_string() const; + + RCLCPP_PUBLIC + const std::vector & + as_byte_array() const; + + RCLCPP_PUBLIC + const std::vector & + as_bool_array() const; + + RCLCPP_PUBLIC + const std::vector & + as_integer_array() const; + + RCLCPP_PUBLIC + const std::vector & + as_double_array() const; + + RCLCPP_PUBLIC + const std::vector & + as_string_array() const; + + RCLCPP_PUBLIC + static Parameter + from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter); + + RCLCPP_PUBLIC + rcl_interfaces::msg::Parameter + to_parameter_msg() const; + + RCLCPP_PUBLIC + std::string + value_to_string() const; + +private: + std::string name_; + ParameterValue value_; +}; + +/// Return a json encoded version of the parameter intended for a dict. +RCLCPP_PUBLIC +std::string +_to_json_dict_entry(const Parameter & param); + +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const rclcpp::Parameter & pv); + +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const std::vector & parameters); + +namespace detail +{ + +template +auto +get_value_helper(const rclcpp::Parameter * parameter) +{ + return parameter->get_parameter_value().get(); +} + +// Specialization allowing Parameter::get() to return a const ref to the parameter value object. +template<> +inline +auto +get_value_helper(const rclcpp::Parameter * parameter) +{ + return parameter->get_parameter_value(); +} + +// Specialization allowing Parameter::get() to return a const ref to the parameter itself. +template<> +inline +auto +get_value_helper(const rclcpp::Parameter * parameter) +{ + // Use this lambda to ensure it's a const reference being returned (and not a copy). + auto type_enforcing_lambda = + [¶meter]() -> const rclcpp::Parameter & { + return *parameter; + }; + return type_enforcing_lambda(); +} + +} // namespace detail + +template +decltype(auto) +Parameter::get_value() const +{ + // use the helper to specialize for the ParameterValue and Parameter cases. + return detail::get_value_helper(this); +} + +} // namespace rclcpp + +namespace std +{ + +/// Return a json encoded version of the parameter intended for a list. +RCLCPP_PUBLIC +std::string +to_string(const rclcpp::Parameter & param); + +/// Return a json encoded version of a vector of parameters, as a string. +RCLCPP_PUBLIC +std::string +to_string(const std::vector & parameters); + +} // namespace std + +#endif // RCLCPP__PARAMETER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_client.hpp new file mode 100644 index 0000000..2cf09d8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_client.hpp @@ -0,0 +1,348 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__PARAMETER_CLIENT_HPP_ +#define RCLCPP__PARAMETER_CLIENT_HPP_ + +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rcl_interfaces/srv/describe_parameters.hpp" +#include "rcl_interfaces/srv/get_parameter_types.hpp" +#include "rcl_interfaces/srv/get_parameters.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +class AsyncParametersClient +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient) + + RCLCPP_PUBLIC + AsyncParametersClient( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + RCLCPP_PUBLIC + AsyncParametersClient( + const rclcpp::Node::SharedPtr node, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + RCLCPP_PUBLIC + AsyncParametersClient( + rclcpp::Node * node, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + RCLCPP_PUBLIC + std::shared_future> + get_parameters( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback = nullptr); + + RCLCPP_PUBLIC + std::shared_future> + get_parameter_types( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback = nullptr); + + RCLCPP_PUBLIC + std::shared_future> + set_parameters( + const std::vector & parameters, + std::function< + void(std::shared_future>) + > callback = nullptr); + + RCLCPP_PUBLIC + std::shared_future + set_parameters_atomically( + const std::vector & parameters, + std::function< + void(std::shared_future) + > callback = nullptr); + + RCLCPP_PUBLIC + std::shared_future + list_parameters( + const std::vector & prefixes, + uint64_t depth, + std::function< + void(std::shared_future) + > callback = nullptr); + + template< + typename CallbackT, + typename AllocatorT = std::allocator> + typename rclcpp::Subscription::SharedPtr + on_parameter_event( + CallbackT && callback, + const rclcpp::QoS & qos = ( + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) + ), + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + )) + { + return this->on_parameter_event( + this->node_topics_interface_, + callback, + qos, + options); + } + + /** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + */ + template< + typename CallbackT, + typename NodeT, + typename AllocatorT = std::allocator> + static typename rclcpp::Subscription::SharedPtr + on_parameter_event( + NodeT && node, + CallbackT && callback, + const rclcpp::QoS & qos = ( + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) + ), + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + )) + { + return rclcpp::create_subscription( + node, + "parameter_events", + qos, + std::forward(callback), + options); + } + + RCLCPP_PUBLIC + bool + service_is_ready() const; + + template + bool + wait_for_service( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return wait_for_service_nanoseconds( + std::chrono::duration_cast(timeout) + ); + } + +protected: + RCLCPP_PUBLIC + bool + wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); + +private: + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_; + rclcpp::Client::SharedPtr get_parameters_client_; + rclcpp::Client::SharedPtr + get_parameter_types_client_; + rclcpp::Client::SharedPtr set_parameters_client_; + rclcpp::Client::SharedPtr + set_parameters_atomically_client_; + rclcpp::Client::SharedPtr list_parameters_client_; + rclcpp::Client::SharedPtr + describe_parameters_client_; + std::string remote_node_name_; +}; + +class SyncParametersClient +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient) + + RCLCPP_PUBLIC + explicit SyncParametersClient( + rclcpp::Node::SharedPtr node, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + + RCLCPP_PUBLIC + SyncParametersClient( + rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Node::SharedPtr node, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + + RCLCPP_PUBLIC + explicit SyncParametersClient( + rclcpp::Node * node, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + + RCLCPP_PUBLIC + SyncParametersClient( + rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Node * node, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + + RCLCPP_PUBLIC + SyncParametersClient( + rclcpp::executor::Executor::SharedPtr executor, + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name = "", + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + + RCLCPP_PUBLIC + std::vector + get_parameters(const std::vector & parameter_names); + + RCLCPP_PUBLIC + bool + has_parameter(const std::string & parameter_name); + + template + T + get_parameter_impl( + const std::string & parameter_name, std::function parameter_not_found_handler) + { + std::vector names; + names.push_back(parameter_name); + auto vars = get_parameters(names); + if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET)) { + return parameter_not_found_handler(); + } else { + return static_cast(vars[0].get_value()); + } + } + + template + T + get_parameter(const std::string & parameter_name, const T & default_value) + { + return get_parameter_impl( + parameter_name, + std::function([&default_value]() -> T {return default_value;})); + } + + template + T + get_parameter(const std::string & parameter_name) + { + return get_parameter_impl( + parameter_name, + std::function([¶meter_name]() -> T + { + throw std::runtime_error("Parameter '" + parameter_name + "' is not set"); + }) + ); + } + + RCLCPP_PUBLIC + std::vector + get_parameter_types(const std::vector & parameter_names); + + RCLCPP_PUBLIC + std::vector + set_parameters(const std::vector & parameters); + + RCLCPP_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically(const std::vector & parameters); + + RCLCPP_PUBLIC + rcl_interfaces::msg::ListParametersResult + list_parameters( + const std::vector & parameter_prefixes, + uint64_t depth); + + template + typename rclcpp::Subscription::SharedPtr + on_parameter_event(CallbackT && callback) + { + return async_parameters_client_->on_parameter_event( + std::forward(callback)); + } + + /** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + */ + template< + typename CallbackT, + typename NodeT> + static typename rclcpp::Subscription::SharedPtr + on_parameter_event( + NodeT && node, + CallbackT && callback) + { + return AsyncParametersClient::on_parameter_event( + node, + std::forward(callback)); + } + + RCLCPP_PUBLIC + bool + service_is_ready() const + { + return async_parameters_client_->service_is_ready(); + } + + template + bool + wait_for_service( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return async_parameters_client_->wait_for_service(timeout); + } + +private: + rclcpp::executor::Executor::SharedPtr executor_; + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; + AsyncParametersClient::SharedPtr async_parameters_client_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_CLIENT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_events_filter.hpp new file mode 100644 index 0000000..d09d287 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -0,0 +1,82 @@ +// Copyright 2017 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 RCLCPP__PARAMETER_EVENTS_FILTER_HPP_ +#define RCLCPP__PARAMETER_EVENTS_FILTER_HPP_ + +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +class ParameterEventsFilter +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter) + enum class EventType {NEW, DELETED, CHANGED}; ///< An enum for the type of event. + /// Used for the listed results + using EventPair = std::pair; + + /// Construct a filtered view of a parameter event. + /** + * \param[in] event The parameter event message to filter. + * \param[in] names A list of parameter names of interest. + * \param[in] types A list of the types of parameter events of iterest. + * EventType NEW, DELETED, or CHANGED + * + * Example Usage: + * + * If you have recieved a parameter event and are only interested in parameters foo and + * bar being added or changed but don't care about deletion. + * + * ```cpp + * auto res = rclcpp::ParameterEventsFilter( + * event_shared_ptr, + * {"foo", "bar"}, + * {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED}); + * ``` + */ + RCLCPP_PUBLIC + ParameterEventsFilter( + rcl_interfaces::msg::ParameterEvent::SharedPtr event, + const std::vector & names, + const std::vector & types); + + /// Get the result of the filter + /** + * \return A std::vector of all matching parameter changes in this event. + */ + RCLCPP_PUBLIC + const std::vector & get_events() const; + +private: + // access only allowed via const accessor. + std::vector result_; ///< Storage of the resultant vector + rcl_interfaces::msg::ParameterEvent::SharedPtr event_; ///< Keep event in scope +}; + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_EVENTS_FILTER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_map.hpp new file mode 100644 index 0000000..09b5977 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_map.hpp @@ -0,0 +1,53 @@ +// Copyright 2018 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 RCLCPP__PARAMETER_MAP_HPP_ +#define RCLCPP__PARAMETER_MAP_HPP_ + +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// A map of fully qualified node names to a list of parameters +using ParameterMap = std::unordered_map>; + +/// Convert parameters from rcl_yaml_param_parser into C++ class instances. +/// \param[in] c_params C structures containing parameters for multiple nodes. +/// \returns a map where the keys are fully qualified node names and values a list of parameters. +/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid. +RCLCPP_PUBLIC +ParameterMap +parameter_map_from(const rcl_params_t * const c_params); + +/// Convert parameter value from rcl_yaml_param_parser into a C++ class instance. +/// \param[in] c_value C structure containing a value of a parameter. +/// \returns an instance of a parameter value +/// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid. +RCLCPP_PUBLIC +ParameterValue +parameter_value_from(const rcl_variant_t * const c_value); + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_MAP_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_service.hpp new file mode 100644 index 0000000..143a522 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_service.hpp @@ -0,0 +1,63 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__PARAMETER_SERVICE_HPP_ +#define RCLCPP__PARAMETER_SERVICE_HPP_ + +#include +#include + +#include "rcl_interfaces/srv/describe_parameters.hpp" +#include "rcl_interfaces/srv/get_parameter_types.hpp" +#include "rcl_interfaces/srv/get_parameters.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +class ParameterService +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ParameterService) + + RCLCPP_PUBLIC + explicit ParameterService( + const std::shared_ptr node_base, + const std::shared_ptr node_services, + rclcpp::node_interfaces::NodeParametersInterface * node_params, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + +private: + rclcpp::Service::SharedPtr get_parameters_service_; + rclcpp::Service::SharedPtr + get_parameter_types_service_; + rclcpp::Service::SharedPtr set_parameters_service_; + rclcpp::Service::SharedPtr + set_parameters_atomically_service_; + rclcpp::Service::SharedPtr + describe_parameters_service_; + rclcpp::Service::SharedPtr list_parameters_service_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_SERVICE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_value.hpp new file mode 100644 index 0000000..77bcdec --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/parameter_value.hpp @@ -0,0 +1,345 @@ +// Copyright 2018 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 RCLCPP__PARAMETER_VALUE_HPP_ +#define RCLCPP__PARAMETER_VALUE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter_type.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +enum ParameterType : uint8_t +{ + PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, + PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + PARAMETER_BOOL_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + PARAMETER_INTEGER_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + PARAMETER_DOUBLE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + PARAMETER_STRING_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, +}; + +/// Return the name of a parameter type +RCLCPP_PUBLIC +std::string +to_string(ParameterType type); + +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, ParameterType type); + +/// Indicate the parameter type does not match the expected type. +class ParameterTypeException : public std::runtime_error +{ +public: + /// Construct an instance. + /** + * \param[in] expected the expected parameter type. + * \param[in] actual the actual parameter type. + */ + RCLCPP_PUBLIC + ParameterTypeException(ParameterType expected, ParameterType actual) + : std::runtime_error("expected [" + to_string(expected) + "] got [" + to_string(actual) + "]") + {} +}; + +/// Store the type and value of a parameter. +class ParameterValue +{ +public: + /// Construct a parameter value with type PARAMETER_NOT_SET. + RCLCPP_PUBLIC + ParameterValue(); + /// Construct a parameter value from a message. + RCLCPP_PUBLIC + explicit ParameterValue(const rcl_interfaces::msg::ParameterValue & value); + /// Construct a parameter value with type PARAMETER_BOOL. + RCLCPP_PUBLIC + explicit ParameterValue(const bool bool_value); + /// Construct a parameter value with type PARAMETER_INTEGER. + RCLCPP_PUBLIC + explicit ParameterValue(const int int_value); + /// Construct a parameter value with type PARAMETER_INTEGER. + RCLCPP_PUBLIC + explicit ParameterValue(const int64_t int_value); + /// Construct a parameter value with type PARAMETER_DOUBLE. + RCLCPP_PUBLIC + explicit ParameterValue(const float double_value); + /// Construct a parameter value with type PARAMETER_DOUBLE. + RCLCPP_PUBLIC + explicit ParameterValue(const double double_value); + /// Construct a parameter value with type PARAMETER_STRING. + RCLCPP_PUBLIC + explicit ParameterValue(const std::string & string_value); + /// Construct a parameter value with type PARAMETER_STRING. + RCLCPP_PUBLIC + explicit ParameterValue(const char * string_value); + /// Construct a parameter value with type PARAMETER_BYTE_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & byte_array_value); + /// Construct a parameter value with type PARAMETER_BOOL_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & bool_array_value); + /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & int_array_value); + /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & int_array_value); + /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & double_array_value); + /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & double_array_value); + /// Construct a parameter value with type PARAMETER_STRING_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & string_array_value); + + /// Return an enum indicating the type of the set value. + RCLCPP_PUBLIC + ParameterType + get_type() const; + + /// Return a message populated with the parameter value + RCLCPP_PUBLIC + rcl_interfaces::msg::ParameterValue + to_value_msg() const; + + /// Equal operator. + RCLCPP_PUBLIC + bool + operator==(const ParameterValue & rhs) const; + + /// Not equal operator. + RCLCPP_PUBLIC + bool + operator!=(const ParameterValue & rhs) const; + + // The following get() variants require the use of ParameterType + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + throw ParameterTypeException(ParameterType::PARAMETER_BOOL, get_type()); + } + return value_.bool_value; + } + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { + throw ParameterTypeException(ParameterType::PARAMETER_INTEGER, get_type()); + } + return value_.integer_value; + } + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { + throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE, get_type()); + } + return value_.double_value; + } + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) { + throw ParameterTypeException(ParameterType::PARAMETER_STRING, get_type()); + } + return value_.string_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_BYTE_ARRAY, get_type()); + } + return value_.byte_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_BOOL_ARRAY, get_type()); + } + return value_.bool_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_INTEGER_ARRAY, get_type()); + } + return value_.integer_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE_ARRAY, get_type()); + } + return value_.double_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_STRING_ARRAY, get_type()); + } + return value_.string_array_value; + } + + // The following get() variants allow the use of primitive types + + template + constexpr + typename std::enable_if::value, const bool &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_integral::value && !std::is_same::value, const int64_t &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if::value, const double &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if::value, const std::string &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + +private: + rcl_interfaces::msg::ParameterValue value_; +}; + +/// Return the value of a parameter as a string +RCLCPP_PUBLIC +std::string +to_string(const ParameterValue & type); + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_VALUE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/publisher.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher.hpp new file mode 100644 index 0000000..b017080 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher.hpp @@ -0,0 +1,350 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__PUBLISHER_HPP_ +#define RCLCPP__PUBLISHER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/publisher.h" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/detail/resolve_use_intra_process.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +template +class LoanedMessage; + +/// A publisher publishes messages of any type to a topic. +template> +class Publisher : public PublisherBase +{ +public: + using MessageAllocatorTraits = allocator::AllocRebind; + using MessageAllocator = typename MessageAllocatorTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) + + Publisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + : PublisherBase( + node_base, + topic, + *rosidl_typesupport_cpp::get_message_type_support_handle(), + options.template to_rcl_publisher_options(qos)), + options_(options), + message_allocator_(new MessageAllocator(*options.get_allocator().get())) + { + allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + + if (options_.event_callbacks.deadline_callback) { + this->add_event_handler( + options_.event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + if (options_.event_callbacks.liveliness_callback) { + this->add_event_handler( + options_.event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + + // Setup continues in the post construction method, post_init_setup(). + } + + /// Called post construction, so that construction may continue after shared_from_this() works. + virtual + void + post_init_setup( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + { + // Topic is unused for now. + (void)topic; + (void)options; + + // If needed, setup intra process communication. + if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { + auto context = node_base->get_context(); + // Get the intra process manager instance for this context. + auto ipm = context->get_sub_context(); + // Register the publisher with the intra process manager. + if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { + throw std::invalid_argument( + "intraprocess communication is not allowed with keep all history qos policy"); + } + if (qos.get_rmw_qos_profile().depth == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with a zero qos history depth value"); + } + if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this()); + this->setup_intra_process( + intra_process_publisher_id, + ipm); + } + } + + virtual ~Publisher() + {} + + /// Borrow a loaned ROS message from the middleware. + /** + * If the middleware is capable of loaning memory for a ROS message instance, + * the loaned message will be directly allocated in the middleware. + * If not, the message allocator of this rclcpp::Publisher instance is being used. + * + * With a call to \sa `publish` the LoanedMessage instance is being returned to the middleware + * or free'd accordingly to the allocator. + * If the message is not being published but processed differently, the destructor of this + * class will either return the message to the middleware or deallocate it via the internal + * allocator. + * \sa rclcpp::LoanedMessage for details of the LoanedMessage class. + * + * \return LoanedMessage containing memory for a ROS message of type MessageT + */ + rclcpp::LoanedMessage + borrow_loaned_message() + { + return rclcpp::LoanedMessage(this, this->get_allocator()); + } + + /// Send a message to the topic for this publisher. + /** + * This function is templated on the input message type, MessageT. + * \param[in] msg A shared pointer to the message to send. + */ + virtual void + publish(std::unique_ptr msg) + { + if (!intra_process_is_enabled_) { + this->do_inter_process_publish(*msg); + return; + } + // If an interprocess subscription exist, then the unique_ptr is promoted + // to a shared_ptr and published. + // This allows doing the intraprocess publish first and then doing the + // interprocess publish, resulting in lower publish-to-subscribe latency. + // It's not possible to do that with an unique_ptr, + // as do_intra_process_publish takes the ownership of the message. + bool inter_process_publish_needed = + get_subscription_count() > get_intra_process_subscription_count(); + + if (inter_process_publish_needed) { + auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + this->do_inter_process_publish(*shared_msg); + } else { + this->do_intra_process_publish(std::move(msg)); + } + } + + virtual void + publish(const MessageT & msg) + { + // Avoid allocating when not using intra process. + if (!intra_process_is_enabled_) { + // In this case we're not using intra process. + return this->do_inter_process_publish(msg); + } + // Otherwise we have to allocate memory in a unique_ptr and pass it along. + // As the message is not const, a copy should be made. + // A shared_ptr could also be constructed here. + auto ptr = MessageAllocatorTraits::allocate(*message_allocator_.get(), 1); + MessageAllocatorTraits::construct(*message_allocator_.get(), ptr, msg); + MessageUniquePtr unique_msg(ptr, message_deleter_); + this->publish(std::move(unique_msg)); + } + + void + publish(const rcl_serialized_message_t & serialized_msg) + { + return this->do_serialized_publish(&serialized_msg); + } + + /// Publish an instance of a LoanedMessage. + /** + * When publishing a loaned message, the memory for this ROS message will be deallocated + * after being published. + * The instance of the loaned message is no longer valid after this call. + * + * \param loaned_msg The LoanedMessage instance to be published. + */ + void + publish(rclcpp::LoanedMessage && loaned_msg) + { + if (!loaned_msg.is_valid()) { + throw std::runtime_error("loaned message is not valid"); + } + if (intra_process_is_enabled_) { + // TODO(Karsten1987): support loaned message passed by intraprocess + throw std::runtime_error("storing loaned messages in intra process is not supported yet"); + } + + // verify that publisher supports loaned messages + // TODO(Karsten1987): This case separation has to be done in rclcpp + // otherwise we have to ensure that every middleware implements + // `rmw_publish_loaned_message` explicitly the same way as `rmw_publish` + // by taking a copy of the ros message. + if (this->can_loan_messages()) { + // we release the ownership from the rclpp::LoanedMessage instance + // and let the middleware clean up the memory. + this->do_loaned_message_publish(loaned_msg.release()); + } else { + // we don't release the ownership, let the middleware copy the ros message + // and thus the destructor of rclcpp::LoanedMessage cleans up the memory. + this->do_inter_process_publish(loaned_msg.get()); + } + } + + std::shared_ptr + get_allocator() const + { + return message_allocator_; + } + +protected: + void + do_inter_process_publish(const MessageT & msg) + { + auto status = rcl_publish(&publisher_handle_, &msg, nullptr); + + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); // next call will reset error message if not context + if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { + rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); + if (nullptr != context && !rcl_context_is_valid(context)) { + // publisher is invalid due to context being shutdown + return; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message"); + } + } + + void + do_serialized_publish(const rcl_serialized_message_t * serialized_msg) + { + if (intra_process_is_enabled_) { + // TODO(Karsten1987): support serialized message passed by intraprocess + throw std::runtime_error("storing serialized messages in intra process is not supported yet"); + } + auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg, nullptr); + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message"); + } + } + + void + do_loaned_message_publish(MessageT * msg) + { + auto status = rcl_publish_loaned_message(&publisher_handle_, msg, nullptr); + + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); // next call will reset error message if not context + if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { + rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); + if (nullptr != context && !rcl_context_is_valid(context)) { + // publisher is invalid due to context being shutdown + return; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message"); + } + } + + void + do_intra_process_publish(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + message_allocator_); + } + + std::shared_ptr + do_intra_process_publish_and_return_shared(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + return ipm->template do_intra_process_publish_and_return_shared( + intra_process_publisher_id_, + std::move(msg), + message_allocator_); + } + + /// Copy of original options passed during construction. + /** + * It is important to save a copy of this so that the rmw payload which it + * may contain is kept alive for the duration of the publisher. + */ + const rclcpp::PublisherOptionsWithAllocator options_; + + std::shared_ptr message_allocator_; + + MessageDeleter message_deleter_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_base.hpp new file mode 100644 index 0000000..4fbcf46 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_base.hpp @@ -0,0 +1,228 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__PUBLISHER_BASE_HPP_ +#define RCLCPP__PUBLISHER_BASE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rcl/publisher.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/qos_event.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +// Forward declaration is used for friend statement. +namespace node_interfaces +{ +class NodeBaseInterface; +class NodeTopicsInterface; +} // namespace node_interfaces + +namespace experimental +{ +/** + * IntraProcessManager is forward declared here, avoiding a circular inclusion between + * `intra_process_manager.hpp` and `publisher_base.hpp`. + */ +class IntraProcessManager; +} // namespace experimental + +class PublisherBase : public std::enable_shared_from_this +{ + friend ::rclcpp::node_interfaces::NodeTopicsInterface; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) + + /// Default constructor. + /** + * Typically, a publisher is not created through this method, but instead is created through a + * call to `Node::create_publisher`. + * \param[in] node_base A pointer to the NodeBaseInterface for the parent node. + * \param[in] topic The topic that this publisher publishes on. + * \param[in] type_support The type support structure for the type to be published. + * \param[in] publisher_options QoS settings for this publisher. + */ + RCLCPP_PUBLIC + PublisherBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rosidl_message_type_support_t & type_support, + const rcl_publisher_options_t & publisher_options); + + RCLCPP_PUBLIC + virtual ~PublisherBase(); + + /// Get the topic that this publisher publishes on. + /** \return The topic name. */ + RCLCPP_PUBLIC + const char * + get_topic_name() const; + + /// Get the queue size for this publisher. + /** \return The queue size. */ + RCLCPP_PUBLIC + size_t + get_queue_size() const; + + /// Get the global identifier for this publisher (used in rmw and by DDS). + /** \return The gid. */ + RCLCPP_PUBLIC + const rmw_gid_t & + get_gid() const; + + /// Get the rcl publisher handle. + /** \return The rcl publisher handle. */ + RCLCPP_PUBLIC + rcl_publisher_t * + get_publisher_handle(); + + /// Get the rcl publisher handle. + /** \return The rcl publisher handle. */ + RCLCPP_PUBLIC + const rcl_publisher_t * + get_publisher_handle() const; + + /// Get all the QoS event handlers associated with this publisher. + /** \return The vector of QoS event handlers. */ + RCLCPP_PUBLIC + const std::vector> & + get_event_handlers() const; + + /// Get subscription count + /** \return The number of subscriptions. */ + RCLCPP_PUBLIC + size_t + get_subscription_count() const; + + /// Get intraprocess subscription count + /** \return The number of intraprocess subscriptions. */ + RCLCPP_PUBLIC + size_t + get_intra_process_subscription_count() const; + + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC). + /** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator + * of this publisher may manually call `assert_liveliness` at some point in time to signal to the + * rest of the system that this Node is still alive. + * + * \return `true` if the liveliness was asserted successfully, otherwise `false` + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + bool + assert_liveliness() const; + + /// Get the actual QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the publisher, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual qos settings. + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_actual_qos() const; + + /// Check if publisher instance can loan messages. + /** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + */ + RCLCPP_PUBLIC + bool + can_loan_messages() const; + + /// Compare this publisher to a gid. + /** + * Note that this function calls the next function. + * \param[in] gid Reference to a gid. + * \return True if the publisher's gid matches the input. + */ + RCLCPP_PUBLIC + bool + operator==(const rmw_gid_t & gid) const; + + /// Compare this publisher to a pointer gid. + /** + * A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal. + * \param[in] gid A pointer to a gid. + * \return True if this publisher's gid matches the input. + */ + RCLCPP_PUBLIC + bool + operator==(const rmw_gid_t * gid) const; + + using IntraProcessManagerSharedPtr = + std::shared_ptr; + + /// Implementation utility function used to setup intra process publishing after creation. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t intra_process_publisher_id, + IntraProcessManagerSharedPtr ipm); + +protected: + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_publisher_event_type_t event_type) + { + auto handler = std::make_shared>( + callback, + rcl_publisher_event_init, + &publisher_handle_, + event_type); + event_handlers_.emplace_back(handler); + } + + std::shared_ptr rcl_node_handle_; + + rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); + + std::vector> event_handlers_; + + using IntraProcessManagerWeakPtr = + std::weak_ptr; + bool intra_process_is_enabled_; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t intra_process_publisher_id_; + + rmw_gid_t rmw_gid_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_BASE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_factory.hpp new file mode 100644 index 0000000..87def3c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_factory.hpp @@ -0,0 +1,91 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__PUBLISHER_FACTORY_HPP_ +#define RCLCPP__PUBLISHER_FACTORY_HPP_ + +#include +#include +#include + +#include "rcl/publisher.h" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Factory with functions used to create a MessageT specific PublisherT. +/** + * This factory class is used to encapsulate the template generated functions + * which are used during the creation of a Message type specific publisher + * within a non-templated class. + * + * It is created using the create_publisher_factory function, which is usually + * called from a templated "create_publisher" method on the Node class, and + * is passed to the non-templated "create_publisher" method on the NodeTopics + * class where it is used to create and setup the Publisher. + * + * It also handles the two step construction of Publishers, first calling + * the constructor and then the post_init_setup() method. + */ +struct PublisherFactory +{ + // Creates a PublisherT publisher object and returns it as a PublisherBase. + using PublisherFactoryFunction = std::function< + rclcpp::PublisherBase::SharedPtr( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos + )>; + + const PublisherFactoryFunction create_typed_publisher; +}; + +/// Return a PublisherFactory with functions setup for creating a PublisherT. +template +PublisherFactory +create_publisher_factory(const rclcpp::PublisherOptionsWithAllocator & options) +{ + PublisherFactory factory { + // factory function that creates a MessageT specific PublisherT + [options]( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos + ) -> std::shared_ptr + { + auto publisher = std::make_shared(node_base, topic_name, qos, options); + // This is used for setting up things like intra process comms which + // require this->shared_from_this() which cannot be called from + // the constructor. + publisher->post_init_setup(node_base, topic_name, qos, options); + return publisher; + } + }; + + // return the factory now that it is populated + return factory; +} + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_FACTORY_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_options.hpp new file mode 100644 index 0000000..cd78d51 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/publisher_options.hpp @@ -0,0 +1,110 @@ +// Copyright 2019 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 RCLCPP__PUBLISHER_OPTIONS_HPP_ +#define RCLCPP__PUBLISHER_OPTIONS_HPP_ + +#include +#include +#include + +#include "rcl/publisher.h" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp" +#include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/qos_event.hpp" + +namespace rclcpp +{ + +namespace callback_group +{ +class CallbackGroup; +} // namespace callback_group + +/// Non-templated part of PublisherOptionsWithAllocator. +struct PublisherOptionsBase +{ + /// Setting to explicitly set intraprocess communications. + IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + + /// Callbacks for various events related to publishers. + PublisherEventCallbacks event_callbacks; + + /// Callback group in which the waitable items from the publisher should be placed. + std::shared_ptr callback_group; + + /// Optional RMW implementation specific payload to be used during creation of the publisher. + std::shared_ptr + rmw_implementation_payload = nullptr; +}; + +/// Structure containing optional configuration for Publishers. +template +struct PublisherOptionsWithAllocator : public PublisherOptionsBase +{ + /// Optional custom allocator. + std::shared_ptr allocator = nullptr; + + PublisherOptionsWithAllocator() {} + + /// Constructor using base class as input. + explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base) + : PublisherOptionsBase(publisher_options_base) + {} + + /// Convert this class, and a rclcpp::QoS, into an rcl_publisher_options_t. + template + rcl_publisher_options_t + to_rcl_publisher_options(const rclcpp::QoS & qos) const + { + rcl_publisher_options_t result; + using AllocatorTraits = std::allocator_traits; + using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; + auto message_alloc = std::make_shared(*this->get_allocator().get()); + result.allocator = rclcpp::allocator::get_rcl_allocator(*message_alloc); + result.qos = qos.get_rmw_qos_profile(); + + // Apply payload to rcl_publisher_options if necessary. + if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) { + rmw_implementation_payload->modify_rmw_publisher_options(result.rmw_publisher_options); + } + + return result; + } + + + /// Get the allocator, creating one if needed. + std::shared_ptr + get_allocator() const + { + if (!this->allocator) { + // TODO(wjwwood): I would like to use the commented line instead, but + // cppcheck 1.89 fails with: + // Syntax Error: AST broken, binary operator '>' doesn't have two operands. + // return std::make_shared(); + std::shared_ptr tmp(new Allocator()); + return tmp; + } + return this->allocator; + } +}; + +using PublisherOptions = PublisherOptionsWithAllocator>; + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_OPTIONS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/qos.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/qos.hpp new file mode 100644 index 0000000..6f7db78 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/qos.hpp @@ -0,0 +1,206 @@ +// Copyright 2019 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 RCLCPP__QOS_HPP_ +#define RCLCPP__QOS_HPP_ + +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead. +struct RCLCPP_PUBLIC QoSInitialization +{ + rmw_qos_history_policy_t history_policy; + size_t depth; + + /// Constructor which takes both a history policy and a depth (even if it would be unused). + QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg); + + /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth. + static + QoSInitialization + from_rmw(const rmw_qos_profile_t & rmw_qos); +}; + +/// Use to initialize the QoS with the keep_all history setting. +struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization +{ + KeepAll(); +}; + +/// Use to initialize the QoS with the keep_last history setting and the given depth. +struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization +{ + explicit KeepLast(size_t depth); +}; + +/// Encapsulation of Quality of Service settings. +class RCLCPP_PUBLIC QoS +{ +public: + /// Constructor which allows you to construct a QoS by giving the only required settings. + explicit + QoS( + const QoSInitialization & qos_initialization, + const rmw_qos_profile_t & initial_profile = rmw_qos_profile_default); + + /// Conversion constructor to ease construction in the common case of just specifying depth. + /** + * Convenience constructor, equivalent to QoS(KeepLast(history_depth)). + */ + // cppcheck-suppress noExplicitConstructor + QoS(size_t history_depth); // NOLINT(runtime/explicit): conversion constructor + + /// Return the rmw qos profile. + rmw_qos_profile_t & + get_rmw_qos_profile(); + + /// Return the rmw qos profile. + const rmw_qos_profile_t & + get_rmw_qos_profile() const; + + /// Set the history policy. + QoS & + history(rmw_qos_history_policy_t history); + + /// Set the history to keep last. + QoS & + keep_last(size_t depth); + + /// Set the history to keep all. + QoS & + keep_all(); + + /// Set the reliability setting. + QoS & + reliability(rmw_qos_reliability_policy_t reliability); + + /// Set the reliability setting to reliable. + QoS & + reliable(); + + /// Set the reliability setting to best effort. + QoS & + best_effort(); + + /// Set the durability setting. + QoS & + durability(rmw_qos_durability_policy_t durability); + + /// Set the durability setting to volatile. + /** + * Note that this cannot be named `volatile` because it is a C++ keyword. + */ + QoS & + durability_volatile(); + + /// Set the durability setting to transient local. + QoS & + transient_local(); + + /// Set the deadline setting. + QoS & + deadline(rmw_time_t deadline); + + /// Set the deadline setting, rclcpp::Duration. + QoS & + deadline(const rclcpp::Duration & deadline); + + /// Set the lifespan setting. + QoS & + lifespan(rmw_time_t lifespan); + + /// Set the lifespan setting, rclcpp::Duration. + QoS & + lifespan(const rclcpp::Duration & lifespan); + + /// Set the liveliness setting. + QoS & + liveliness(rmw_qos_liveliness_policy_t liveliness); + + /// Set the liveliness_lease_duration setting. + QoS & + liveliness_lease_duration(rmw_time_t liveliness_lease_duration); + + /// Set the liveliness_lease_duration setting, rclcpp::Duration. + QoS & + liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration); + + /// Set the avoid_ros_namespace_conventions setting. + QoS & + avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions); + +private: + rmw_qos_profile_t rmw_qos_profile_; +}; + +class RCLCPP_PUBLIC SensorDataQoS : public QoS +{ +public: + explicit + SensorDataQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_sensor_data) + )); +}; + +class RCLCPP_PUBLIC ParametersQoS : public QoS +{ +public: + explicit + ParametersQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_parameters) + )); +}; + +class RCLCPP_PUBLIC ServicesQoS : public QoS +{ +public: + explicit + ServicesQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_services_default) + )); +}; + +class RCLCPP_PUBLIC ParameterEventsQoS : public QoS +{ +public: + explicit + ParameterEventsQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) + )); +}; + +class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS +{ +public: + explicit + SystemDefaultsQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_system_default) + )); +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/qos_event.hpp new file mode 100644 index 0000000..d14af2b --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/qos_event.hpp @@ -0,0 +1,126 @@ +// Copyright 2019 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 RCLCPP__QOS_EVENT_HPP_ +#define RCLCPP__QOS_EVENT_HPP_ + +#include + +#include "rcl/error_handling.h" + +#include "rcutils/logging_macros.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/function_traits.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ + +using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; +using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; +using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; +using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; + +using QOSDeadlineRequestedCallbackType = std::function; +using QOSDeadlineOfferedCallbackType = std::function; +using QOSLivelinessChangedCallbackType = std::function; +using QOSLivelinessLostCallbackType = std::function; + +/// Contains callbacks for various types of events a Publisher can receive from the middleware. +struct PublisherEventCallbacks +{ + QOSDeadlineOfferedCallbackType deadline_callback; + QOSLivelinessLostCallbackType liveliness_callback; +}; + +/// Contains callbacks for non-message events that a Subscription can receive from the middleware. +struct SubscriptionEventCallbacks +{ + QOSDeadlineRequestedCallbackType deadline_callback; + QOSLivelinessChangedCallbackType liveliness_callback; +}; + +class QOSEventHandlerBase : public Waitable +{ +public: + RCLCPP_PUBLIC + virtual ~QOSEventHandlerBase(); + + /// Get the number of ready events + RCLCPP_PUBLIC + size_t + get_number_of_ready_events() override; + + /// Add the Waitable to a wait set. + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + /// Check if the Waitable is ready. + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + +protected: + rcl_event_t event_handle_; + size_t wait_set_event_index_; +}; + +template +class QOSEventHandler : public QOSEventHandlerBase +{ +public: + template + QOSEventHandler( + const EventCallbackT & callback, + InitFuncT init_func, + ParentHandleT parent_handle, + EventTypeEnum event_type) + : event_callback_(callback) + { + event_handle_ = rcl_get_zero_initialized_event(); + rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event"); + } + } + + /// Execute any entities of the Waitable that are ready. + void + execute() override + { + EventCallbackInfoT callback_info; + + rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't take event info: %s", rcl_get_error_string().str); + return; + } + + event_callback_(callback_info); + } + +private: + using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + + EventCallbackT event_callback_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_EVENT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/rate.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/rate.hpp new file mode 100644 index 0000000..296cce1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/rate.hpp @@ -0,0 +1,119 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__RATE_HPP_ +#define RCLCPP__RATE_HPP_ + +#include +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +class RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase) + + virtual bool sleep() = 0; + virtual bool is_steady() const = 0; + virtual void reset() = 0; +}; + +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + +template +class GenericRate : public RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) + + explicit GenericRate(double rate) + : GenericRate( + duration_cast(duration(1.0 / rate))) + {} + explicit GenericRate(std::chrono::nanoseconds period) + : period_(period), last_interval_(Clock::now()) + {} + + virtual bool + sleep() + { + // Time coming into sleep + auto now = Clock::now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (time_to_sleep <= std::chrono::seconds(0)) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + rclcpp::sleep_for(time_to_sleep); + return true; + } + + virtual bool + is_steady() const + { + return Clock::is_steady; + } + + virtual void + reset() + { + last_interval_ = Clock::now(); + } + + std::chrono::nanoseconds period() const + { + return period_; + } + +private: + RCLCPP_DISABLE_COPY(GenericRate) + + std::chrono::nanoseconds period_; + using ClockDurationNano = std::chrono::duration; + std::chrono::time_point last_interval_; +}; + +using Rate = GenericRate; +using WallRate = GenericRate; + +} // namespace rclcpp + +#endif // RCLCPP__RATE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/rclcpp.hpp new file mode 100644 index 0000000..98de3cb --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/rclcpp.hpp @@ -0,0 +1,157 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** \mainpage rclcpp: ROS Client Library for C++ + * + * `rclcpp` provides the canonical C++ API for interacting with ROS. + * It consists of these main components: + * + * - Node + * - rclcpp::Node + * - rclcpp/node.hpp + * - Publisher + * - rclcpp::Node::create_publisher() + * - rclcpp::Publisher + * - rclcpp::Publisher::publish() + * - rclcpp/publisher.hpp + * - Subscription + * - rclcpp::Node::create_subscription() + * - rclcpp::Subscription + * - rclcpp/subscription.hpp + * - Service Client + * - rclcpp::Node::create_client() + * - rclcpp::Client + * - rclcpp/client.hpp + * - Service Server + * - rclcpp::Node::create_service() + * - rclcpp::Service + * - rclcpp/service.hpp + * - Timer + * - rclcpp::Node::create_wall_timer() + * - rclcpp::WallTimer + * - rclcpp::TimerBase + * - rclcpp/timer.hpp + * - Parameters: + * - rclcpp::Node::set_parameters() + * - rclcpp::Node::get_parameters() + * - rclcpp::Node::get_parameter() + * - rclcpp::Node::describe_parameters() + * - rclcpp::Node::list_parameters() + * - rclcpp::Node::add_on_set_parameters_callback() + * - rclcpp::Node::remove_on_set_parameters_callback() + * - rclcpp::Parameter + * - rclcpp::ParameterValue + * - rclcpp::AsyncParametersClient + * - rclcpp::SyncParametersClient + * - rclcpp/parameter.hpp + * - rclcpp/parameter_value.hpp + * - rclcpp/parameter_client.hpp + * - rclcpp/parameter_service.hpp + * - Rate: + * - rclcpp::Rate + * - rclcpp::WallRate + * - rclcpp/rate.hpp + * + * There are also some components which help control the execution of callbacks: + * + * - Executors (responsible for execution of callbacks through a blocking spin): + * - rclcpp::spin() + * - rclcpp::spin_some() + * - rclcpp::spin_until_future_complete() + * - rclcpp::executors::SingleThreadedExecutor + * - rclcpp::executors::SingleThreadedExecutor::add_node() + * - rclcpp::executors::SingleThreadedExecutor::spin() + * - rclcpp::executors::MultiThreadedExecutor + * - rclcpp::executors::MultiThreadedExecutor::add_node() + * - rclcpp::executors::MultiThreadedExecutor::spin() + * - rclcpp/executor.hpp + * - rclcpp/executors.hpp + * - rclcpp/executors/single_threaded_executor.hpp + * - rclcpp/executors/multi_threaded_executor.hpp + * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks): + * - rclcpp::Node::create_callback_group() + * - rclcpp::callback_group::CallbackGroup + * - rclcpp/callback_group.hpp + * + * Additionally, there are some methods for introspecting the ROS graph: + * + * - Graph Events (a waitable event object that wakes up when the graph changes): + * - rclcpp::Node::get_graph_event() + * - rclcpp::Node::wait_for_graph_change() + * - rclcpp::Event + * - List topic names and types: + * - rclcpp::Node::get_topic_names_and_types() + * - Get the number of publishers or subscribers on a topic: + * - rclcpp::Node::count_publishers() + * - rclcpp::Node::count_subscribers() + * + * And components related to logging: + * + * - Logging macros: + * - Some examples (not exhaustive): + * - RCLCPP_DEBUG() + * - RCLCPP_INFO() + * - RCLCPP_WARN_ONCE() + * - RCLCPP_ERROR_SKIPFIRST() + * - rclcpp/logging.hpp + * - Logger: + * - rclcpp::Logger + * - rclcpp/logger.hpp + * - rclcpp::Node::get_logger() + * + * Finally, there are many internal API's and utilities: + * + * - Exceptions: + * - rclcpp/exceptions.hpp + * - Allocator related items: + * - rclcpp/allocator/allocator_common.hpp + * - rclcpp/allocator/allocator_deleter.hpp + * - Memory management tools: + * - rclcpp/memory_strategies.hpp + * - rclcpp/memory_strategy.hpp + * - rclcpp/message_memory_strategy.hpp + * - rclcpp/strategies/allocator_memory_strategy.hpp + * - rclcpp/strategies/message_pool_memory_strategy.hpp + * - Context object which is shared amongst multiple Nodes: + * - rclcpp::Context + * - rclcpp/context.hpp + * - rclcpp/contexts/default_context.hpp + * - Various utilities: + * - rclcpp/function_traits.hpp + * - rclcpp/macros.hpp + * - rclcpp/scope_exit.hpp + * - rclcpp/time.hpp + * - rclcpp/utilities.hpp + * - rclcpp/visibility_control.hpp + */ + +#ifndef RCLCPP__RCLCPP_HPP_ +#define RCLCPP__RCLCPP_HPP_ + +#include +#include + +#include "rclcpp/executors.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/parameter_service.hpp" +#include "rclcpp/rate.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +#endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/scope_exit.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/scope_exit.hpp new file mode 100644 index 0000000..7865de5 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/scope_exit.hpp @@ -0,0 +1,52 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/ +// But I changed the lambda to include by reference rather than value, see: +// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873 + +#ifndef RCLCPP__SCOPE_EXIT_HPP_ +#define RCLCPP__SCOPE_EXIT_HPP_ + +#include + +#include "rclcpp/macros.hpp" + +namespace rclcpp +{ + +template +struct ScopeExit +{ + explicit ScopeExit(Callable callable) + : callable_(callable) {} + ~ScopeExit() {callable_();} + +private: + Callable callable_; +}; + +template +ScopeExit +make_scope_exit(Callable callable) +{ + return ScopeExit(callable); +} + +} // namespace rclcpp + +#define RCLCPP_SCOPE_EXIT(code) \ + auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;}) + +#endif // RCLCPP__SCOPE_EXIT_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/service.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/service.hpp new file mode 100644 index 0000000..1386845 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/service.hpp @@ -0,0 +1,260 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SERVICE_HPP_ +#define RCLCPP__SERVICE_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/service.h" + +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/logging.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ + +class ServiceBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) + + RCLCPP_PUBLIC + explicit ServiceBase( + std::shared_ptr node_handle); + + RCLCPP_PUBLIC + virtual ~ServiceBase(); + + RCLCPP_PUBLIC + const char * + get_service_name(); + + RCLCPP_PUBLIC + std::shared_ptr + get_service_handle(); + + RCLCPP_PUBLIC + std::shared_ptr + get_service_handle() const; + + virtual std::shared_ptr create_request() = 0; + virtual std::shared_ptr create_request_header() = 0; + virtual void handle_request( + std::shared_ptr request_header, + std::shared_ptr request) = 0; + +protected: + RCLCPP_DISABLE_COPY(ServiceBase) + + RCLCPP_PUBLIC + rcl_node_t * + get_rcl_node_handle(); + + RCLCPP_PUBLIC + const rcl_node_t * + get_rcl_node_handle() const; + + std::shared_ptr node_handle_; + + std::shared_ptr service_handle_; + bool owns_rcl_handle_ = true; +}; + +template +class Service : public ServiceBase +{ +public: + using CallbackType = std::function< + void ( + const std::shared_ptr, + std::shared_ptr)>; + + using CallbackWithHeaderType = std::function< + void ( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr)>; + RCLCPP_SMART_PTR_DEFINITIONS(Service) + + Service( + std::shared_ptr node_handle, + const std::string & service_name, + AnyServiceCallback any_callback, + rcl_service_options_t & service_options) + : ServiceBase(node_handle), any_callback_(any_callback) + { + using rosidl_typesupport_cpp::get_service_type_support_handle; + auto service_type_support_handle = get_service_type_support_handle(); + + std::weak_ptr weak_node_handle(node_handle_); + // rcl does the static memory allocation here + service_handle_ = std::shared_ptr( + new rcl_service_t, [weak_node_handle](rcl_service_t * service) + { + auto handle = weak_node_handle.lock(); + if (handle) { + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Error in destruction of rcl service handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete service; + }); + *service_handle_.get() = rcl_get_zero_initialized_service(); + + rcl_ret_t ret = rcl_service_init( + service_handle_.get(), + node_handle.get(), + service_type_support_handle, + service_name.c_str(), + &service_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); + } + TRACEPOINT( + rclcpp_service_callback_added, + (const void *)get_service_handle().get(), + (const void *)&any_callback_); + any_callback_.register_callback_for_tracing(); + } + + Service( + std::shared_ptr node_handle, + std::shared_ptr service_handle, + AnyServiceCallback any_callback) + : ServiceBase(node_handle), + any_callback_(any_callback) + { + // check if service handle was initialized + if (!rcl_service_is_valid(service_handle.get())) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("rcl_service_t in constructor argument must be initialized beforehand.")); + // *INDENT-ON* + } + + service_handle_ = service_handle; + TRACEPOINT( + rclcpp_service_callback_added, + (const void *)get_service_handle().get(), + (const void *)&any_callback_); + any_callback_.register_callback_for_tracing(); + } + + Service( + std::shared_ptr node_handle, + rcl_service_t * service_handle, + AnyServiceCallback any_callback) + : ServiceBase(node_handle), + any_callback_(any_callback) + { + // check if service handle was initialized + if (!rcl_service_is_valid(service_handle)) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("rcl_service_t in constructor argument must be initialized beforehand.")); + // *INDENT-ON* + } + + // In this case, rcl owns the service handle memory + service_handle_ = std::shared_ptr(new rcl_service_t); + service_handle_->impl = service_handle->impl; + TRACEPOINT( + rclcpp_service_callback_added, + (const void *)get_service_handle().get(), + (const void *)&any_callback_); + any_callback_.register_callback_for_tracing(); + } + + Service() = delete; + + virtual ~Service() + { + } + + std::shared_ptr create_request() + { + return std::shared_ptr(new typename ServiceT::Request()); + } + + std::shared_ptr create_request_header() + { + // TODO(wjwwood): This should probably use rmw_request_id's allocator. + // (since it is a C type) + return std::shared_ptr(new rmw_request_id_t); + } + + void handle_request( + std::shared_ptr request_header, + std::shared_ptr request) + { + auto typed_request = std::static_pointer_cast(request); + auto response = std::shared_ptr(new typename ServiceT::Response); + any_callback_.dispatch(request_header, typed_request, response); + send_response(request_header, response); + } + + void send_response( + std::shared_ptr req_id, + std::shared_ptr response) + { + rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get()); + + if (status != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response"); + } + } + +private: + RCLCPP_DISABLE_COPY(Service) + + AnyServiceCallback any_callback_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SERVICE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp new file mode 100644 index 0000000..339e5a0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -0,0 +1,508 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_ +#define RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_ + +#include +#include + +#include "rcl/allocator.h" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcutils/logging_macros.h" + +#include "rmw/types.h" + +namespace rclcpp +{ +namespace memory_strategies +{ +namespace allocator_memory_strategy +{ + +/// Delegate for handling memory allocations while the Executor is executing. +/** + * By default, the memory strategy dynamically allocates memory for structures that come in from + * the rmw implementation after the executor waits for work, based on the number of entities that + * come through. + */ +template> +class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy) + + using VoidAllocTraits = typename allocator::AllocRebind; + using VoidAlloc = typename VoidAllocTraits::allocator_type; + + explicit AllocatorMemoryStrategy(std::shared_ptr allocator) + { + allocator_ = std::make_shared(*allocator.get()); + } + + AllocatorMemoryStrategy() + { + allocator_ = std::make_shared(); + } + + void add_guard_condition(const rcl_guard_condition_t * guard_condition) + { + for (const auto & existing_guard_condition : guard_conditions_) { + if (existing_guard_condition == guard_condition) { + return; + } + } + guard_conditions_.push_back(guard_condition); + } + + void remove_guard_condition(const rcl_guard_condition_t * guard_condition) + { + for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { + if (*it == guard_condition) { + guard_conditions_.erase(it); + break; + } + } + } + + void clear_handles() + { + subscription_handles_.clear(); + service_handles_.clear(); + client_handles_.clear(); + timer_handles_.clear(); + waitable_handles_.clear(); + } + + virtual void remove_null_handles(rcl_wait_set_t * wait_set) + { + // TODO(jacobperron): Check if wait set sizes are what we expect them to be? + // e.g. wait_set->size_of_clients == client_handles_.size() + + // Important to use subscription_handles_.size() instead of wait set's size since + // there may be more subscriptions in the wait set due to Waitables added to the end. + // The same logic applies for other entities. + for (size_t i = 0; i < subscription_handles_.size(); ++i) { + if (!wait_set->subscriptions[i]) { + subscription_handles_[i].reset(); + } + } + for (size_t i = 0; i < service_handles_.size(); ++i) { + if (!wait_set->services[i]) { + service_handles_[i].reset(); + } + } + for (size_t i = 0; i < client_handles_.size(); ++i) { + if (!wait_set->clients[i]) { + client_handles_[i].reset(); + } + } + for (size_t i = 0; i < timer_handles_.size(); ++i) { + if (!wait_set->timers[i]) { + timer_handles_[i].reset(); + } + } + for (size_t i = 0; i < waitable_handles_.size(); ++i) { + if (!waitable_handles_[i]->is_ready(wait_set)) { + waitable_handles_[i].reset(); + } + } + + subscription_handles_.erase( + std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr), + subscription_handles_.end() + ); + + service_handles_.erase( + std::remove(service_handles_.begin(), service_handles_.end(), nullptr), + service_handles_.end() + ); + + client_handles_.erase( + std::remove(client_handles_.begin(), client_handles_.end(), nullptr), + client_handles_.end() + ); + + timer_handles_.erase( + std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), + timer_handles_.end() + ); + + waitable_handles_.erase( + std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), + waitable_handles_.end() + ); + } + + bool collect_entities(const WeakNodeList & weak_nodes) + { + bool has_invalid_weak_nodes = false; + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + has_invalid_weak_nodes = true; + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group || !group->can_be_taken_from().load()) { + continue; + } + group->find_subscription_ptrs_if( + [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { + subscription_handles_.push_back(subscription->get_subscription_handle()); + return false; + }); + + group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) { + service_handles_.push_back(service->get_service_handle()); + return false; + }); + group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) { + client_handles_.push_back(client->get_client_handle()); + return false; + }); + group->find_timer_ptrs_if([this](const rclcpp::TimerBase::SharedPtr & timer) { + timer_handles_.push_back(timer->get_timer_handle()); + return false; + }); + group->find_waitable_ptrs_if([this](const rclcpp::Waitable::SharedPtr & waitable) { + waitable_handles_.push_back(waitable); + return false; + }); + } + } + return has_invalid_weak_nodes; + } + + bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) + { + for (auto subscription : subscription_handles_) { + if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + for (auto client : client_handles_) { + if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add client to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + for (auto service : service_handles_) { + if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add service to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + for (auto timer : timer_handles_) { + if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add timer to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + for (auto guard_condition : guard_conditions_) { + if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add guard_condition to wait set: %s", + rcl_get_error_string().str); + return false; + } + } + + for (auto waitable : waitable_handles_) { + if (!waitable->add_to_wait_set(wait_set)) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add waitable to wait set: %s", rcl_get_error_string().str); + return false; + } + } + return true; + } + + virtual void + get_next_subscription( + executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) + { + auto it = subscription_handles_.begin(); + while (it != subscription_handles_.end()) { + auto subscription = get_subscription_by_handle(*it, weak_nodes); + if (subscription) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_subscription(subscription, weak_nodes); + if (!group) { + // Group was not found, meaning the subscription is not valid... + // Remove it from the ready list and continue looking + it = subscription_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.subscription = subscription; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + subscription_handles_.erase(it); + return; + } + // Else, the subscription is no longer valid, remove it and continue + it = subscription_handles_.erase(it); + } + } + + virtual void + get_next_service( + executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) + { + auto it = service_handles_.begin(); + while (it != service_handles_.end()) { + auto service = get_service_by_handle(*it, weak_nodes); + if (service) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_service(service, weak_nodes); + if (!group) { + // Group was not found, meaning the service is not valid... + // Remove it from the ready list and continue looking + it = service_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.service = service; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + service_handles_.erase(it); + return; + } + // Else, the service is no longer valid, remove it and continue + it = service_handles_.erase(it); + } + } + + virtual void + get_next_client(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) + { + auto it = client_handles_.begin(); + while (it != client_handles_.end()) { + auto client = get_client_by_handle(*it, weak_nodes); + if (client) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_client(client, weak_nodes); + if (!group) { + // Group was not found, meaning the service is not valid... + // Remove it from the ready list and continue looking + it = client_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.client = client; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + client_handles_.erase(it); + return; + } + // Else, the service is no longer valid, remove it and continue + it = client_handles_.erase(it); + } + } + + virtual void + get_next_timer( + executor::AnyExecutable & any_exec, + const WeakNodeList & weak_nodes) + { + auto it = timer_handles_.begin(); + while (it != timer_handles_.end()) { + auto timer = get_timer_by_handle(*it, weak_nodes); + if (timer) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_timer(timer, weak_nodes); + if (!group) { + // Group was not found, meaning the timer is not valid... + // Remove it from the ready list and continue looking + it = timer_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.timer = timer; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + timer_handles_.erase(it); + return; + } + // Else, the service is no longer valid, remove it and continue + it = timer_handles_.erase(it); + } + } + + virtual void + get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes) + { + auto it = waitable_handles_.begin(); + while (it != waitable_handles_.end()) { + auto waitable = *it; + if (waitable) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_waitable(waitable, weak_nodes); + if (!group) { + // Group was not found, meaning the waitable is not valid... + // Remove it from the ready list and continue looking + it = waitable_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.waitable = waitable; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + waitable_handles_.erase(it); + return; + } + // Else, the waitable is no longer valid, remove it and continue + it = waitable_handles_.erase(it); + } + } + + virtual rcl_allocator_t get_allocator() + { + return rclcpp::allocator::get_rcl_allocator(*allocator_.get()); + } + + size_t number_of_ready_subscriptions() const + { + size_t number_of_subscriptions = subscription_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); + } + return number_of_subscriptions; + } + + size_t number_of_ready_services() const + { + size_t number_of_services = service_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_services += waitable->get_number_of_ready_services(); + } + return number_of_services; + } + + size_t number_of_ready_events() const + { + size_t number_of_events = 0; + for (auto waitable : waitable_handles_) { + number_of_events += waitable->get_number_of_ready_events(); + } + return number_of_events; + } + + size_t number_of_ready_clients() const + { + size_t number_of_clients = client_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_clients += waitable->get_number_of_ready_clients(); + } + return number_of_clients; + } + + size_t number_of_guard_conditions() const + { + size_t number_of_guard_conditions = guard_conditions_.size(); + for (auto waitable : waitable_handles_) { + number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions(); + } + return number_of_guard_conditions; + } + + size_t number_of_ready_timers() const + { + size_t number_of_timers = timer_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_timers += waitable->get_number_of_ready_timers(); + } + return number_of_timers; + } + + size_t number_of_waitables() const + { + return waitable_handles_.size(); + } + +private: + template + using VectorRebind = + std::vector::template rebind_alloc>; + + VectorRebind guard_conditions_; + + VectorRebind> subscription_handles_; + VectorRebind> service_handles_; + VectorRebind> client_handles_; + VectorRebind> timer_handles_; + VectorRebind> waitable_handles_; + + std::shared_ptr allocator_; +}; + +} // namespace allocator_memory_strategy +} // namespace memory_strategies +} // namespace rclcpp + +#endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp new file mode 100644 index 0000000..0eb1d54 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -0,0 +1,112 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ +#define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ + +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace strategies +{ +namespace message_pool_memory_strategy +{ + +/// Completely static memory allocation strategy for messages. +/** + * Templated on the type of message pooled by this class and the size of the message pool. + * Templating allows the program to determine the memory required for this object at compile time. + * The size of the message pool should be at least the largest number of concurrent accesses to + * the subscription (usually the number of threads). + */ +template< + typename MessageT, + size_t Size, + typename std::enable_if< + rosidl_generator_traits::has_fixed_size::value + >::type * = nullptr +> +class MessagePoolMemoryStrategy + : public message_memory_strategy::MessageMemoryStrategy +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy) + + /// Default constructor + MessagePoolMemoryStrategy() + : next_array_index_(0) + { + for (size_t i = 0; i < Size; ++i) { + pool_[i].msg_ptr_ = std::make_shared(); + pool_[i].used = false; + } + } + + /// Borrow a message from the message pool. + /** + * Manage the message pool ring buffer. + * Throw an exception if the next message was not available. + * \return Shared pointer to the borrowed message. + */ + std::shared_ptr borrow_message() + { + size_t current_index = next_array_index_; + next_array_index_ = (next_array_index_ + 1) % Size; + if (pool_[current_index].used) { + throw std::runtime_error("Tried to access message that was still in use! Abort."); + } + pool_[current_index].msg_ptr_->~MessageT(); + new (pool_[current_index].msg_ptr_.get())MessageT; + + pool_[current_index].used = true; + return pool_[current_index].msg_ptr_; + } + + /// Return a message to the message pool. + /** + * Manage metadata in the message pool ring buffer to release the message. + * \param[in] msg Shared pointer to the message to return. + */ + void return_message(std::shared_ptr & msg) + { + for (size_t i = 0; i < Size; ++i) { + if (pool_[i].msg_ptr_ == msg) { + pool_[i].used = false; + return; + } + } + throw std::runtime_error("Unrecognized message ptr in return_message."); + } + +protected: + struct PoolMember + { + std::shared_ptr msg_ptr_; + bool used; + }; + + std::array pool_; + size_t next_array_index_; +}; + +} // namespace message_pool_memory_strategy +} // namespace strategies +} // namespace rclcpp + +#endif // RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/subscription.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription.hpp new file mode 100644 index 0000000..4a33d81 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription.hpp @@ -0,0 +1,276 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_HPP_ +#define RCLCPP__SUBSCRIPTION_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include + + +#include "rcl/error_handling.h" +#include "rcl/subscription.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/detail/resolve_use_intra_process.hpp" +#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/experimental/subscription_intra_process.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ + +namespace node_interfaces +{ +class NodeTopicsInterface; +} // namespace node_interfaces + +/// Subscription implementation, templated on the type of message this subscription receives. +template< + typename CallbackMessageT, + typename AllocatorT = std::allocator, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + AllocatorT + >> +class Subscription : public SubscriptionBase +{ + friend class rclcpp::node_interfaces::NodeTopicsInterface; + +public: + using MessageAllocatorTraits = allocator::AllocRebind; + using MessageAllocator = typename MessageAllocatorTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(Subscription) + + /// Default constructor. + /** + * The constructor for a subscription is almost never called directly. + * Instead, subscriptions should be instantiated through the function + * rclcpp::create_subscription(). + * + * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. + * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. + * \param[in] topic_name Name of the topic to subscribe to. + * \param[in] callback User defined callback to call when a message is received. + * \param[in] options options for the subscription. + * \param[in] message_memory_strategy The memory strategy to be used for managing message memory. + */ + Subscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, + const std::string & topic_name, + const rclcpp::QoS & qos, + AnySubscriptionCallback callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr message_memory_strategy) + : SubscriptionBase( + node_base, + type_support_handle, + topic_name, + options.template to_rcl_subscription_options(qos), + rclcpp::subscription_traits::is_serialized_subscription_argument::value), + any_callback_(callback), + options_(options), + message_memory_strategy_(message_memory_strategy) + { + if (options.event_callbacks.deadline_callback) { + this->add_event_handler( + options.event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + if (options.event_callbacks.liveliness_callback) { + this->add_event_handler( + options.event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + + // Setup intra process publishing if requested. + if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { + using rclcpp::detail::resolve_intra_process_buffer_type; + + // Check if the QoS is compatible with intra-process. + rmw_qos_profile_t qos_profile = get_actual_qos().get_rmw_qos_profile(); + if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { + throw std::invalid_argument( + "intraprocess communication is not allowed with keep all history qos policy"); + } + if (qos_profile.depth == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + if (qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + + // First create a SubscriptionIntraProcess which will be given to the intra-process manager. + auto context = node_base->get_context(); + auto subscription_intra_process = std::make_shared< + rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type + >>( + callback, + options.get_allocator(), + context, + this->get_topic_name(), // important to get like this, as it has the fully-qualified name + qos_profile, + resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) + ); + TRACEPOINT( + rclcpp_subscription_init, + (const void *)get_subscription_handle().get(), + (const void *)subscription_intra_process.get()); + + // Add it to the intra process manager. + using rclcpp::experimental::IntraProcessManager; + auto ipm = context->get_sub_context(); + uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process); + this->setup_intra_process(intra_process_subscription_id, ipm); + } + + TRACEPOINT( + rclcpp_subscription_init, + (const void *)get_subscription_handle().get(), + (const void *)this); + TRACEPOINT( + rclcpp_subscription_callback_added, + (const void *)this, + (const void *)&any_callback_); + // The callback object gets copied, so if registration is done too early/before this point + // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later + // in subsequent tracepoints. + any_callback_.register_callback_for_tracing(); + } + + /// Called after construction to continue setup that requires shared_from_this(). + void post_init_setup( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rclcpp::QoS & qos, + const rclcpp::SubscriptionOptionsWithAllocator & options) + { + (void)node_base; + (void)qos; + (void)options; + } + + /// Support dynamically setting the message memory strategy. + /** + * Behavior may be undefined if called while the subscription could be executing. + * \param[in] message_memory_strategy Shared pointer to the memory strategy to set. + */ + void set_message_memory_strategy( + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) + { + message_memory_strategy_ = message_memory_strategy; + } + + std::shared_ptr create_message() override + { + /* The default message memory strategy provides a dynamically allocated message on each call to + * create_message, though alternative memory strategies that re-use a preallocated message may be + * used (see rclcpp/strategies/message_pool_memory_strategy.hpp). + */ + return message_memory_strategy_->borrow_message(); + } + + std::shared_ptr create_serialized_message() override + { + return message_memory_strategy_->borrow_serialized_message(); + } + + void handle_message( + std::shared_ptr & message, const rmw_message_info_t & message_info) override + { + if (matches_any_intra_process_publishers(&message_info.publisher_gid)) { + // In this case, the message will be delivered via intra process and + // we should ignore this copy of the message. + return; + } + auto typed_message = std::static_pointer_cast(message); + any_callback_.dispatch(typed_message, message_info); + } + + void + handle_loaned_message( + void * loaned_message, const rmw_message_info_t & message_info) override + { + auto typed_message = static_cast(loaned_message); + // message is loaned, so we have to make sure that the deleter does not deallocate the message + auto sptr = std::shared_ptr( + typed_message, [](CallbackMessageT * msg) {(void) msg;}); + any_callback_.dispatch(sptr, message_info); + } + + /// Return the borrowed message. + /** \param message message to be returned */ + void return_message(std::shared_ptr & message) override + { + auto typed_message = std::static_pointer_cast(message); + message_memory_strategy_->return_message(typed_message); + } + + void return_serialized_message(std::shared_ptr & message) override + { + message_memory_strategy_->return_serialized_message(message); + } + + bool use_take_shared_method() const + { + return any_callback_.use_take_shared_method(); + } + +private: + RCLCPP_DISABLE_COPY(Subscription) + + AnySubscriptionCallback any_callback_; + /// Copy of original options passed during construction. + /** + * It is important to save a copy of this so that the rmw payload which it + * may contain is kept alive for the duration of the subscription. + */ + const rclcpp::SubscriptionOptionsWithAllocator options_; + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr + message_memory_strategy_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_base.hpp new file mode 100644 index 0000000..23ecb5e --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_base.hpp @@ -0,0 +1,236 @@ +// Copyright 2019 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 RCLCPP__SUBSCRIPTION_BASE_HPP_ +#define RCLCPP__SUBSCRIPTION_BASE_HPP_ + +#include +#include +#include + +#include "rcl/subscription.h" + +#include "rmw/rmw.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/qos_event.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +namespace node_interfaces +{ +class NodeBaseInterface; +} // namespace node_interfaces + +namespace experimental +{ +/** + * IntraProcessManager is forward declared here, avoiding a circular inclusion between + * `intra_process_manager.hpp` and `subscription_base.hpp`. + */ +class IntraProcessManager; +} // namespace experimental + +/// Virtual base class for subscriptions. This pattern allows us to iterate over different template +/// specializations of Subscription, among other things. +class SubscriptionBase : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase) + + /// Default constructor. + /** + * \param[in] node_base NodeBaseInterface pointer used in parts of the setup. + * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. + * \param[in] topic_name Name of the topic to subscribe to. + * \param[in] subscription_options options for the subscription. + * \param[in] is_serialized is true if the message will be delivered still serialized + */ + RCLCPP_PUBLIC + SubscriptionBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, + const std::string & topic_name, + const rcl_subscription_options_t & subscription_options, + bool is_serialized = false); + + /// Default destructor. + RCLCPP_PUBLIC + virtual ~SubscriptionBase(); + + /// Get the topic that this subscription is subscribed on. + RCLCPP_PUBLIC + const char * + get_topic_name() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_subscription_handle(); + + RCLCPP_PUBLIC + const std::shared_ptr + get_subscription_handle() const; + + /// Get all the QoS event handlers associated with this subscription. + /** \return The vector of QoS event handlers. */ + RCLCPP_PUBLIC + const std::vector> & + get_event_handlers() const; + + /// Get the actual QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the publisher, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual qos settings. + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_actual_qos() const; + + /// Borrow a new message. + /** \return Shared pointer to the fresh message. */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + create_message() = 0; + + /// Borrow a new serialized message + /** \return Shared pointer to a rcl_message_serialized_t. */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + create_serialized_message() = 0; + + /// Check if we need to handle the message, and execute the callback if we do. + /** + * \param[in] message Shared pointer to the message to handle. + * \param[in] message_info Metadata associated with this message. + */ + RCLCPP_PUBLIC + virtual + void + handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; + + RCLCPP_PUBLIC + virtual + void + handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0; + + /// Return the message borrowed in create_message. + /** \param[in] message Shared pointer to the returned message. */ + RCLCPP_PUBLIC + virtual + void + return_message(std::shared_ptr & message) = 0; + + /// Return the message borrowed in create_serialized_message. + /** \param[in] message Shared pointer to the returned message. */ + RCLCPP_PUBLIC + virtual + void + return_serialized_message(std::shared_ptr & message) = 0; + + RCLCPP_PUBLIC + const rosidl_message_type_support_t & + get_message_type_support_handle() const; + + RCLCPP_PUBLIC + bool + is_serialized() const; + + /// Get matching publisher count. + /** \return The number of publishers on this topic. */ + RCLCPP_PUBLIC + size_t + get_publisher_count() const; + + /// Check if subscription instance can loan messages. + /** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + * + * \return boolean flag indicating if middleware can loan messages. + */ + RCLCPP_PUBLIC + bool + can_loan_messages() const; + + using IntraProcessManagerWeakPtr = + std::weak_ptr; + + /// Implemenation detail. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t intra_process_subscription_id, + IntraProcessManagerWeakPtr weak_ipm); + + /// Return the waitable for intra-process, or nullptr if intra-process is not setup. + RCLCPP_PUBLIC + rclcpp::Waitable::SharedPtr + get_intra_process_waitable() const; + +protected: + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_subscription_event_type_t event_type) + { + auto handler = std::make_shared>( + callback, + rcl_subscription_event_init, + get_subscription_handle().get(), + event_type); + event_handlers_.emplace_back(handler); + } + + RCLCPP_PUBLIC + bool + matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const; + + rclcpp::node_interfaces::NodeBaseInterface * const node_base_; + + std::shared_ptr node_handle_; + std::shared_ptr subscription_handle_; + std::shared_ptr intra_process_subscription_handle_; + + std::vector> event_handlers_; + + bool use_intra_process_; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t intra_process_subscription_id_; + +private: + RCLCPP_DISABLE_COPY(SubscriptionBase) + + rosidl_message_type_support_t type_support_; + bool is_serialized_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_BASE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_factory.hpp new file mode 100644 index 0000000..a0f265c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_factory.hpp @@ -0,0 +1,123 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_FACTORY_HPP_ +#define RCLCPP__SUBSCRIPTION_FACTORY_HPP_ + +#include +#include +#include +#include + +#include "rcl/subscription.h" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Factory containing a function used to create a Subscription. +/** + * This factory class is used to encapsulate the template generated function + * which is used during the creation of a Message type specific subscription + * within a non-templated class. + * + * It is created using the create_subscription_factory function, which is + * usually called from a templated "create_subscription" method of the Node + * class, and is passed to the non-templated "create_subscription" method of + * the NodeTopics class where it is used to create and setup the Subscription. + * + * It also handles the two step construction of Subscriptions, first calling + * the constructor and then the post_init_setup() method. + */ +struct SubscriptionFactory +{ + // Creates a Subscription object and returns it as a SubscriptionBase. + using SubscriptionFactoryFunction = std::function< + rclcpp::SubscriptionBase::SharedPtr( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos)>; + + const SubscriptionFactoryFunction create_typed_subscription; +}; + +/// Return a SubscriptionFactory setup to create a SubscriptionT. +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + AllocatorT + >> +SubscriptionFactory +create_subscription_factory( + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) +{ + auto allocator = options.get_allocator(); + + using rclcpp::AnySubscriptionCallback; + AnySubscriptionCallback any_subscription_callback(allocator); + any_subscription_callback.set(std::forward(callback)); + + SubscriptionFactory factory { + // factory function that creates a MessageT specific SubscriptionT + [options, msg_mem_strat, any_subscription_callback]( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos + ) -> rclcpp::SubscriptionBase::SharedPtr + { + using rclcpp::Subscription; + using rclcpp::SubscriptionBase; + + auto sub = Subscription::make_shared( + node_base, + *rosidl_typesupport_cpp::get_message_type_support_handle(), + topic_name, + qos, + any_subscription_callback, + options, + msg_mem_strat); + // This is used for setting up things like intra process comms which + // require this->shared_from_this() which cannot be called from + // the constructor. + sub->post_init_setup(node_base, qos, options); + auto sub_base_ptr = std::dynamic_pointer_cast(sub); + return sub_base_ptr; + } + }; + + // return the factory now that it is populated + return factory; +} + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_FACTORY_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_options.hpp new file mode 100644 index 0000000..64fcac5 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_options.hpp @@ -0,0 +1,107 @@ +// Copyright 2019 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 RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ +#define RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ + +#include +#include +#include + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" +#include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/qos_event.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Non-template base class for subscription options. +struct SubscriptionOptionsBase +{ + /// Callbacks for events related to this subscription. + SubscriptionEventCallbacks event_callbacks; + + /// True to ignore local publications. + bool ignore_local_publications = false; + + /// The callback group for this subscription. NULL to use the default callback group. + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr; + + /// Setting to explicitly set intraprocess communications. + IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + + /// Setting the data-type stored in the intraprocess buffer + IntraProcessBufferType intra_process_buffer_type = IntraProcessBufferType::CallbackDefault; + + /// Optional RMW implementation specific payload to be used during creation of the subscription. + std::shared_ptr + rmw_implementation_payload = nullptr; +}; + +/// Structure containing optional configuration for Subscriptions. +template +struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase +{ + /// Optional custom allocator. + std::shared_ptr allocator = nullptr; + + SubscriptionOptionsWithAllocator() {} + + /// Constructor using base class as input. + explicit SubscriptionOptionsWithAllocator( + const SubscriptionOptionsBase & subscription_options_base) + : SubscriptionOptionsBase(subscription_options_base) + {} + + /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t. + template + rcl_subscription_options_t + to_rcl_subscription_options(const rclcpp::QoS & qos) const + { + rcl_subscription_options_t result = rcl_subscription_get_default_options(); + using AllocatorTraits = std::allocator_traits; + using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; + auto message_alloc = std::make_shared(*allocator.get()); + result.allocator = allocator::get_rcl_allocator(*message_alloc); + result.qos = qos.get_rmw_qos_profile(); + result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications; + + // Apply payload to rcl_subscription_options if necessary. + if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) { + rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); + } + + return result; + } + + /// Get the allocator, creating one if needed. + std::shared_ptr + get_allocator() const + { + if (!this->allocator) { + return std::make_shared(); + } + return this->allocator; + } +}; + +using SubscriptionOptions = SubscriptionOptionsWithAllocator>; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_traits.hpp new file mode 100644 index 0000000..ecab458 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/subscription_traits.hpp @@ -0,0 +1,97 @@ +// Copyright 2017 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 RCLCPP__SUBSCRIPTION_TRAITS_HPP_ +#define RCLCPP__SUBSCRIPTION_TRAITS_HPP_ + +#include + +#include "rclcpp/function_traits.hpp" +#include "rcl/types.h" + +namespace rclcpp +{ + +class QoS; + +namespace subscription_traits +{ + +/* + * The current version of uncrustify has a misinterpretion here + * between `:` used for inheritance vs for initializer list + * The result is that whenever a templated struct is used, + * the colon has to be without any whitespace next to it whereas + * when no template is used, the colon has to be separated by a space. + * Cheers! + */ +template +struct is_serialized_subscription_argument : std::false_type +{}; + +template<> +struct is_serialized_subscription_argument: std::true_type +{}; + +template<> +struct is_serialized_subscription_argument> + : std::true_type +{}; + +template +struct is_serialized_subscription : is_serialized_subscription_argument +{}; + +template +struct is_serialized_callback + : is_serialized_subscription_argument< + typename rclcpp::function_traits::function_traits::template argument_type<0>> +{}; + +template +struct extract_message_type +{ + using type = typename std::remove_cv::type; +}; + +template +struct extract_message_type>: extract_message_type +{}; + +template +struct extract_message_type>: extract_message_type +{}; + +template< + typename CallbackT, + // Do not attempt if CallbackT is an integer (mistaken for depth) + typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a QoS (mistaken for qos) + typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile) + typename = std::enable_if_t>>::value> +> +struct has_message_type : extract_message_type< + typename rclcpp::function_traits::function_traits::template argument_type<0>> +{}; + +} // namespace subscription_traits +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_TRAITS_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/time.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/time.hpp new file mode 100644 index 0000000..c10b6ce --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/time.hpp @@ -0,0 +1,130 @@ +// Copyright 2017 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 RCLCPP__TIME_HPP_ +#define RCLCPP__TIME_HPP_ + +#include "builtin_interfaces/msg/time.hpp" + +#include "rclcpp/visibility_control.hpp" + +#include "rcl/time.h" + +#include "rclcpp/duration.hpp" + +namespace rclcpp +{ + +class Clock; + +class Time +{ +public: + RCLCPP_PUBLIC + Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); + + RCLCPP_PUBLIC + explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME); + + RCLCPP_PUBLIC + Time(const Time & rhs); + + RCLCPP_PUBLIC + Time( + const builtin_interfaces::msg::Time & time_msg, + rcl_clock_type_t ros_time = RCL_ROS_TIME); + + RCLCPP_PUBLIC + explicit Time(const rcl_time_point_t & time_point); + + RCLCPP_PUBLIC + virtual ~Time(); + + RCLCPP_PUBLIC + operator builtin_interfaces::msg::Time() const; + + RCLCPP_PUBLIC + Time & + operator=(const Time & rhs); + + RCLCPP_PUBLIC + Time & + operator=(const builtin_interfaces::msg::Time & time_msg); + + RCLCPP_PUBLIC + bool + operator==(const rclcpp::Time & rhs) const; + + RCLCPP_PUBLIC + bool + operator!=(const rclcpp::Time & rhs) const; + + RCLCPP_PUBLIC + bool + operator<(const rclcpp::Time & rhs) const; + + RCLCPP_PUBLIC + bool + operator<=(const rclcpp::Time & rhs) const; + + RCLCPP_PUBLIC + bool + operator>=(const rclcpp::Time & rhs) const; + + RCLCPP_PUBLIC + bool + operator>(const rclcpp::Time & rhs) const; + + RCLCPP_PUBLIC + Time + operator+(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + Duration + operator-(const rclcpp::Time & rhs) const; + + RCLCPP_PUBLIC + Time + operator-(const rclcpp::Duration & rhs) const; + + RCLCPP_PUBLIC + rcl_time_point_value_t + nanoseconds() const; + + RCLCPP_PUBLIC + static Time + max(); + + /// \return the seconds since epoch as a floating point number. + /// \warning Depending on sizeof(double) there could be significant precision loss. + /// When an exact time is required use nanoseconds() instead. + RCLCPP_PUBLIC + double + seconds() const; + + RCLCPP_PUBLIC + rcl_clock_type_t + get_clock_type() const; + +private: + rcl_time_point_t rcl_time_; + friend Clock; // Allow clock to manipulate internal data +}; + +Time +operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); + +} // namespace rclcpp + +#endif // RCLCPP__TIME_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/time_source.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/time_source.hpp new file mode 100644 index 0000000..9dd4023 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/time_source.hpp @@ -0,0 +1,143 @@ +// Copyright 2017 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 RCLCPP__TIME_SOURCE_HPP_ +#define RCLCPP__TIME_SOURCE_HPP_ + +#include +#include + +#include "rcl/time.h" + +#include "builtin_interfaces/msg/time.hpp" +#include "rosgraph_msgs/msg/clock.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" + + +namespace rclcpp +{ +class Clock; + +class TimeSource +{ +public: + RCLCPP_PUBLIC + explicit TimeSource(rclcpp::Node::SharedPtr node); + + RCLCPP_PUBLIC + TimeSource(); + + RCLCPP_PUBLIC + void attachNode(rclcpp::Node::SharedPtr node); + + RCLCPP_PUBLIC + void attachNode( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface); + + RCLCPP_PUBLIC + void detachNode(); + + /// Attach a clock to the time source to be updated + /** + * \throws std::invalid_argument if node is nullptr + */ + RCLCPP_PUBLIC + void attachClock(rclcpp::Clock::SharedPtr clock); + + RCLCPP_PUBLIC + void detachClock(rclcpp::Clock::SharedPtr clock); + + RCLCPP_PUBLIC + ~TimeSource(); + +private: + // Preserve the node reference + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + + // Store (and update on node attach) logger for logging. + Logger logger_; + + // The subscription for the clock callback + using MessageT = rosgraph_msgs::msg::Clock; + using Alloc = std::allocator; + using SubscriptionT = rclcpp::Subscription; + std::shared_ptr clock_subscription_; + std::mutex clock_sub_lock_; + + // The clock callback itself + void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg); + + // Create the subscription for the clock topic + void create_clock_sub(); + + // Destroy the subscription for the clock topic + void destroy_clock_sub(); + + // Parameter Event subscription + using ParamMessageT = rcl_interfaces::msg::ParameterEvent; + using ParamSubscriptionT = rclcpp::Subscription; + std::shared_ptr parameter_subscription_; + + // Callback for parameter updates + void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + + // An enum to hold the parameter state + enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; + UseSimTimeParameterState parameter_state_; + + // An internal method to use in the clock callback that iterates and enables all clocks + void enable_ros_time(); + // An internal method to use in the clock callback that iterates and disables all clocks + void disable_ros_time(); + + // Internal helper functions used inside iterators + static void enable_ros_time(rclcpp::Clock::SharedPtr clock); + static void disable_ros_time(rclcpp::Clock::SharedPtr clock); + static void set_clock( + const builtin_interfaces::msg::Time::SharedPtr msg, + bool set_ros_time_enabled, + rclcpp::Clock::SharedPtr clock); + + // Local storage of validity of ROS time + // This is needed when new clocks are added. + bool ros_time_active_; + // Last set message to be passed to newly registered clocks + rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_; + + // A lock to protect iterating the associated_clocks_ field. + std::mutex clock_list_lock_; + // A vector to store references to associated clocks. + std::vector associated_clocks_; + // A handler for the use_sim_time parameter callback. + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_ = nullptr; +}; + +} // namespace rclcpp + +#endif // RCLCPP__TIME_SOURCE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/timer.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/timer.hpp new file mode 100644 index 0000000..afeb63b --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/timer.hpp @@ -0,0 +1,233 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TIMER_HPP_ +#define RCLCPP__TIMER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/function_traits.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/rate.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" + +#include "rcl/error_handling.h" +#include "rcl/timer.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +namespace rclcpp +{ + +class TimerBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase) + + RCLCPP_PUBLIC + explicit TimerBase( + Clock::SharedPtr clock, + std::chrono::nanoseconds period, + rclcpp::Context::SharedPtr context); + + RCLCPP_PUBLIC + ~TimerBase(); + + RCLCPP_PUBLIC + void + cancel(); + + /// Return the timer cancellation state. + /** + * \return true if the timer has been cancelled, false otherwise + * \throws std::runtime_error if the rcl_get_error_state returns 0 + * \throws RCLErrorBase some child class exception based on ret + */ + RCLCPP_PUBLIC + bool + is_canceled(); + + RCLCPP_PUBLIC + void + reset(); + + RCLCPP_PUBLIC + virtual void + execute_callback() = 0; + + RCLCPP_PUBLIC + std::shared_ptr + get_timer_handle(); + + /// Check how long the timer has until its next scheduled callback. + /** \return A std::chrono::duration representing the relative time until the next callback. */ + RCLCPP_PUBLIC + std::chrono::nanoseconds + time_until_trigger(); + + /// Is the clock steady (i.e. is the time between ticks constant?) + /** \return True if the clock used by this timer is steady. */ + virtual bool is_steady() = 0; + + /// Check if the timer is ready to trigger the callback. + /** + * This function expects its caller to immediately trigger the callback after this function, + * since it maintains the last time the callback was triggered. + * \return True if the timer needs to trigger. + */ + RCLCPP_PUBLIC + bool is_ready(); + +protected: + Clock::SharedPtr clock_; + std::shared_ptr timer_handle_; +}; + + +using VoidCallbackType = std::function; +using TimerCallbackType = std::function; + +/// Generic timer. Periodically executes a user-specified callback. +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value + >::type * = nullptr +> +class GenericTimer : public TimerBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer) + + /// Default constructor. + /** + * \param[in] clock The clock providing the current time. + * \param[in] period The interval at which the timer fires. + * \param[in] callback User-specified callback function. + */ + explicit GenericTimer( + Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback, + rclcpp::Context::SharedPtr context + ) + : TimerBase(clock, period, context), callback_(std::forward(callback)) + { + TRACEPOINT( + rclcpp_timer_callback_added, + (const void *)get_timer_handle().get(), + (const void *)&callback_); + TRACEPOINT( + rclcpp_callback_register, + (const void *)&callback_, + get_symbol(callback_)); + } + + /// Default destructor. + virtual ~GenericTimer() + { + // Stop the timer from running. + cancel(); + } + + void + execute_callback() override + { + rcl_ret_t ret = rcl_timer_call(timer_handle_.get()); + if (ret == RCL_RET_TIMER_CANCELED) { + return; + } + if (ret != RCL_RET_OK) { + throw std::runtime_error("Failed to notify timer that callback occurred"); + } + TRACEPOINT(callback_start, (const void *)&callback_, false); + execute_callback_delegate<>(); + TRACEPOINT(callback_end, (const void *)&callback_); + } + + // void specialization + template< + typename CallbackT = FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value + >::type * = nullptr + > + void + execute_callback_delegate() + { + callback_(); + } + + template< + typename CallbackT = FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value + >::type * = nullptr + > + void + execute_callback_delegate() + { + callback_(*this); + } + + bool + is_steady() override + { + return clock_->get_clock_type() == RCL_STEADY_TIME; + } + +protected: + RCLCPP_DISABLE_COPY(GenericTimer) + + FunctorT callback_; +}; + +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value + >::type * = nullptr +> +class WallTimer : public GenericTimer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(WallTimer) + + WallTimer( + std::chrono::nanoseconds period, + FunctorT && callback, + rclcpp::Context::SharedPtr context) + : GenericTimer( + std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context) + {} + +protected: + RCLCPP_DISABLE_COPY(WallTimer) +}; + +} // namespace rclcpp + +#endif // RCLCPP__TIMER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/type_support_decl.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/type_support_decl.hpp new file mode 100644 index 0000000..cda0bae --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/type_support_decl.hpp @@ -0,0 +1,78 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TYPE_SUPPORT_DECL_HPP_ +#define RCLCPP__TYPE_SUPPORT_DECL_HPP_ + +#include "rosidl_generator_cpp/message_type_support_decl.hpp" +#include "rosidl_generator_cpp/service_type_support_decl.hpp" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_cpp/service_type_support.hpp" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace type_support +{ + +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_intra_process_message_msg_type_support(); + +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_parameter_event_msg_type_support(); + +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_set_parameters_result_msg_type_support(); + +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_parameter_descriptor_msg_type_support(); + +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_list_parameters_result_msg_type_support(); + +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_get_parameters_srv_type_support(); + +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_get_parameter_types_srv_type_support(); + +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_set_parameters_srv_type_support(); + +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_list_parameters_srv_type_support(); + +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_describe_parameters_srv_type_support(); + +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_set_parameters_atomically_srv_type_support(); + +} // namespace type_support +} // namespace rclcpp + +#endif // RCLCPP__TYPE_SUPPORT_DECL_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/utilities.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/utilities.hpp new file mode 100644 index 0000000..606636e --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/utilities.hpp @@ -0,0 +1,307 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__UTILITIES_HPP_ +#define RCLCPP__UTILITIES_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/context.hpp" +#include "rclcpp/init_options.hpp" +#include "rclcpp/visibility_control.hpp" + +#ifdef ANDROID +#include + +namespace std +{ +template +std::string to_string(T value) +{ + std::ostringstream os; + os << value; + return os.str(); +} +} +#endif + +namespace rclcpp +{ +/// Initialize communications via the rmw implementation and set up a global signal handler. +/** + * Initializes the global context which is accessible via the function + * rclcpp::contexts::default_context::get_global_default_context(). + * Also, installs the global signal handlers with the function + * rclcpp::install_signal_handlers(). + * + * \sa rclcpp::Context::init() for more details on arguments and possible exceptions + */ +RCLCPP_PUBLIC +void +init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions()); + +/// Install the global signal handler for rclcpp. +/** + * This function should only need to be run one time per process. + * It is implicitly run by rclcpp::init(), and therefore this function does not + * need to be run manually if rclcpp::init() has already been run. + * + * The signal handler will shutdown all initialized context. + * It will also interrupt any blocking functions in ROS allowing them react to + * any changes in the state of the system (like shutdown). + * + * This function is thread-safe. + * + * \return true if signal handler was installed by this function, false if already installed. + */ +RCLCPP_PUBLIC +bool +install_signal_handlers(); + +/// Return true if the signal handlers are installed, otherwise false. +RCLCPP_PUBLIC +bool +signal_handlers_installed(); + +/// Uninstall the global signal handler for rclcpp. +/** + * This function does not necessarily need to be called, but can be used to + * undo what rclcpp::install_signal_handlers() or rclcpp::init() do with + * respect to signal handling. + * If you choose to use it, this function only needs to be run one time. + * It is implicitly run by rclcpp::shutdown(), and therefore this function does + * not need to be run manually if rclcpp::shutdown() has already been run. + * + * This function is thread-safe. + * + * \return true if signal handler was uninstalled by this function, false if was not installed. + */ +RCLCPP_PUBLIC +bool +uninstall_signal_handlers(); + +/// Initialize communications via the rmw implementation and set up a global signal handler. +/** + * Additionally removes ROS-specific arguments from the argument vector. + * + * \sa rclcpp::Context::init() for more details on arguments and possible exceptions + * \returns Members of the argument vector that are not ROS arguments. + * \throws anything remove_ros_arguments can throw + */ +RCLCPP_PUBLIC +std::vector +init_and_remove_ros_arguments( + int argc, + char const * const argv[], + const InitOptions & init_options = InitOptions()); + +/// Remove ROS-specific arguments from argument vector. +/** + * Some arguments may not have been intended as ROS arguments. + * This function populates the arguments in a vector. + * Since the first argument is always assumed to be a process name, the vector + * will always contain the process name. + * + * \param[in] argc Number of arguments. + * \param[in] argv Argument vector. + * \returns Members of the argument vector that are not ROS arguments. + * \throws anything throw_from_rcl_error can throw + * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails + */ +RCLCPP_PUBLIC +std::vector +remove_ros_arguments(int argc, char const * const argv[]); + +/// Check rclcpp's status. +/** + * This may return false for a context which has been shutdown, or for a + * context that was shutdown due to SIGINT being received by the rclcpp signal + * handler. + * + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * \param[in] context Check for shutdown of this Context. + * \return true if shutdown has been called, false otherwise + */ +RCLCPP_PUBLIC +bool +ok(rclcpp::Context::SharedPtr context = nullptr); + +/// Return true if init() has already been called for the given context. +/** + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * Deprecated, as it is no longer different from rcl_ok(). + * + * \param[in] context Check for initialization of this Context. + * \return true if the context is initialized, and false otherwise + */ +RCLCPP_PUBLIC +bool +is_initialized(rclcpp::Context::SharedPtr context = nullptr); + +/// Shutdown rclcpp context, invalidating it for derived entities. +/** + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * If the global context is used, then the signal handlers are also uninstalled. + * + * This will also cause the "on_shutdown" callbacks to be called. + * + * \sa rclcpp::Context::shutdown() + * \param[in] context to be shutdown + * \return true if shutdown was successful, false if context was already shutdown + */ +RCLCPP_PUBLIC +bool +shutdown( + rclcpp::Context::SharedPtr context = nullptr, + const std::string & reason = "user called rclcpp::shutdown()"); + +/// Register a function to be called when shutdown is called on the context. +/** + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * These callbacks are called when the associated Context is shutdown with the + * Context::shutdown() method. + * When shutdown by the SIGINT handler, shutdown, and therefore these callbacks, + * is called asynchronously from the dedicated signal handling thread, at some + * point after the SIGINT signal is received. + * + * \sa rclcpp::Context::on_shutdown() + * \param[in] callback to be called when the given context is shutdown + * \param[in] context with which to associate the context + */ +RCLCPP_PUBLIC +void +on_shutdown(std::function callback, rclcpp::Context::SharedPtr context = nullptr); + +/// Use the global condition variable to block for the specified amount of time. +/** + * This function can be interrupted early if the associated context becomes + * invalid due to shutdown() or the signal handler. + * \sa rclcpp::Context::sleep_for + * + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. + * \param[in] context which may interrupt this sleep + * \return true if the condition variable did not timeout. + */ +RCLCPP_PUBLIC +bool +sleep_for( + const std::chrono::nanoseconds & nanoseconds, + rclcpp::Context::SharedPtr context = nullptr); + +/// Safely check if addition will overflow. +/** + * The type of the operands, T, should have defined + * std::numeric_limits::max(), `>`, `<` and `-` operators. + * + * \param[in] x is the first addend. + * \param[in] y is the second addend. + * \tparam T is type of the operands. + * \return True if the x + y sum is greater than T::max value. + */ +template +bool +add_will_overflow(const T x, const T y) +{ + return (y > 0) && (x > (std::numeric_limits::max() - y)); +} + +/// Safely check if addition will underflow. +/** + * The type of the operands, T, should have defined + * std::numeric_limits::min(), `>`, `<` and `-` operators. + * + * \param[in] x is the first addend. + * \param[in] y is the second addend. + * \tparam T is type of the operands. + * \return True if the x + y sum is less than T::min value. + */ +template +bool +add_will_underflow(const T x, const T y) +{ + return (y < 0) && (x < (std::numeric_limits::min() - y)); +} + +/// Safely check if subtraction will overflow. +/** + * The type of the operands, T, should have defined + * std::numeric_limits::max(), `>`, `<` and `+` operators. + * + * \param[in] x is the minuend. + * \param[in] y is the subtrahend. + * \tparam T is type of the operands. + * \return True if the difference `x - y` sum is grater than T::max value. + */ +template +bool +sub_will_overflow(const T x, const T y) +{ + return (y < 0) && (x > (std::numeric_limits::max() + y)); +} + +/// Safely check if subtraction will underflow. +/** + * The type of the operands, T, should have defined + * std::numeric_limits::min(), `>`, `<` and `+` operators. + * + * \param[in] x is the minuend. + * \param[in] y is the subtrahend. + * \tparam T is type of the operands. + * \return True if the difference `x - y` sum is less than T::min value. + */ +template +bool +sub_will_underflow(const T x, const T y) +{ + return (y > 0) && (x < (std::numeric_limits::min() + y)); +} + +/// Return the given string. +/** + * This function is overloaded to transform any string to C-style string. + * + * \param[in] string_in is the string to be returned + * \return the given string + */ +RCLCPP_PUBLIC +const char * +get_c_string(const char * string_in); + +/// Return the C string from the given std::string. +/** + * \param[in] string_in is a std::string + * \return the C string from the std::string + */ +RCLCPP_PUBLIC +const char * +get_c_string(const std::string & string_in); + +} // namespace rclcpp + +#endif // RCLCPP__UTILITIES_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/visibility_control.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/visibility_control.hpp new file mode 100644 index 0000000..044fedb --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCLCPP__VISIBILITY_CONTROL_HPP_ +#define RCLCPP__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCLCPP_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_IMPORT __attribute__ ((dllimport)) + #else + #define RCLCPP_EXPORT __declspec(dllexport) + #define RCLCPP_IMPORT __declspec(dllimport) + #endif + #ifdef RCLCPP_BUILDING_LIBRARY + #define RCLCPP_PUBLIC RCLCPP_EXPORT + #else + #define RCLCPP_PUBLIC RCLCPP_IMPORT + #endif + #define RCLCPP_PUBLIC_TYPE RCLCPP_PUBLIC + #define RCLCPP_LOCAL +#else + #define RCLCPP_EXPORT __attribute__ ((visibility("default"))) + #define RCLCPP_IMPORT + #if __GNUC__ >= 4 + #define RCLCPP_PUBLIC __attribute__ ((visibility("default"))) + #define RCLCPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCLCPP_PUBLIC + #define RCLCPP_LOCAL + #endif + #define RCLCPP_PUBLIC_TYPE +#endif + +#endif // RCLCPP__VISIBILITY_CONTROL_HPP_ diff --git a/rclcpp-eloquent/rclcpp/include/rclcpp/waitable.hpp b/rclcpp-eloquent/rclcpp/include/rclcpp/waitable.hpp new file mode 100644 index 0000000..2837d5a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/include/rclcpp/waitable.hpp @@ -0,0 +1,153 @@ +// Copyright 2018 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 RCLCPP__WAITABLE_HPP_ +#define RCLCPP__WAITABLE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl/wait.h" + +namespace rclcpp +{ + +class Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable) + + /// Get the number of ready subscriptions + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more subscriptions. + * \return The number of subscriptions associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_subscriptions(); + + /// Get the number of ready timers + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more timers. + * \return The number of timers associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_timers(); + + /// Get the number of ready clients + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more clients. + * \return The number of clients associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_clients(); + + /// Get the number of ready events + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more events. + * \return The number of events associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_events(); + + /// Get the number of ready services + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more services. + * \return The number of services associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_services(); + + /// Get the number of ready guard_conditions + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more guard_conditions. + * \return The number of guard_conditions associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_guard_conditions(); + + // TODO(jacobperron): smart pointer? + /// Add the Waitable to a wait set. + /** + * \param[in] wait_set A handle to the wait set to add the Waitable to. + * \return `true` if the Waitable is added successfully, `false` otherwise. + * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*() + */ + RCLCPP_PUBLIC + virtual + bool + add_to_wait_set(rcl_wait_set_t * wait_set) = 0; + + /// Check if the Waitable is ready. + /** + * The input wait set should be the same that was used in a previously call to + * `add_to_wait_set()`. + * The wait set should also have been previously waited on with `rcl_wait()`. + * + * \param[in] wait_set A handle to the wait set the Waitable was previously added to + * and that has been waited on. + * \return `true` if the Waitable is ready, `false` otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + is_ready(rcl_wait_set_t *) = 0; + + /// Execute any entities of the Waitable that are ready. + /** + * Before calling this method, the Waitable should be added to a wait set, + * waited on, and then updated. + * + * Example usage: + * + * ```cpp + * // ... create a wait set and a Waitable + * // Add the Waitable to the wait set + * bool add_ret = waitable.add_to_wait_set(wait_set); + * // ... error handling + * // Wait + * rcl_ret_t wait_ret = rcl_wait(wait_set); + * // ... error handling + * // Update the Waitable + * waitable.update(wait_set); + * // Execute any entities of the Waitable that may be ready + * waitable.execute(); + * ``` + */ + RCLCPP_PUBLIC + virtual + void + execute() = 0; +}; // class Waitable + +} // namespace rclcpp + +#endif // RCLCPP__WAITABLE_HPP_ diff --git a/rclcpp-eloquent/rclcpp/package.xml b/rclcpp-eloquent/rclcpp/package.xml new file mode 100644 index 0000000..a036132 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/package.xml @@ -0,0 +1,46 @@ + + + + rclcpp + 0.8.5 + The ROS client library in C++. + Dirk Thomas + Apache License 2.0 + + ament_cmake_ros + + rmw + + builtin_interfaces + rcl_interfaces + rosgraph_msgs + rosidl_generator_cpp + rosidl_typesupport_c + rosidl_typesupport_cpp + builtin_interfaces + rcl_interfaces + rosgraph_msgs + rosidl_generator_cpp + rosidl_typesupport_c + rosidl_typesupport_cpp + + rcl + rcl_yaml_param_parser + rcpputils + rmw_implementation + tracetools + + ament_cmake + + ament_cmake_gmock + ament_cmake_gtest + ament_lint_auto + ament_lint_common + rmw + rmw_implementation_cmake + test_msgs + + + ament_cmake + + diff --git a/rclcpp-eloquent/rclcpp/resource/logging.hpp.em b/rclcpp-eloquent/rclcpp/resource/logging.hpp.em new file mode 100644 index 0000000..9d5ef1a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/resource/logging.hpp.em @@ -0,0 +1,166 @@ +// generated from rclcpp/resource/logging.hpp.em + +// Copyright 2017 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 RCLCPP__LOGGING_HPP_ +#define RCLCPP__LOGGING_HPP_ + +#include + +#include "rclcpp/logger.hpp" +#include "rcutils/logging_macros.h" +#include "rclcpp/utilities.hpp" + +// These are used for compiling out logging macros lower than a minimum severity. +#define RCLCPP_LOG_MIN_SEVERITY_DEBUG 0 +#define RCLCPP_LOG_MIN_SEVERITY_INFO 1 +#define RCLCPP_LOG_MIN_SEVERITY_WARN 2 +#define RCLCPP_LOG_MIN_SEVERITY_ERROR 3 +#define RCLCPP_LOG_MIN_SEVERITY_FATAL 4 +#define RCLCPP_LOG_MIN_SEVERITY_NONE 5 + +#define RCLCPP_FIRST_ARG(N, ...) N +#define RCLCPP_ALL_BUT_FIRST_ARGS(N, ...) __VA_ARGS__ + +/** + * \def RCLCPP_LOG_MIN_SEVERITY + * Define RCLCPP_LOG_MIN_SEVERITY=RCLCPP_LOG_MIN_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL] + * in your build options to compile out anything below that severity. + * Use RCLCPP_LOG_MIN_SEVERITY_NONE to compile out all macros. + */ +#ifndef RCLCPP_LOG_MIN_SEVERITY +#define RCLCPP_LOG_MIN_SEVERITY RCLCPP_LOG_MIN_SEVERITY_DEBUG +#endif + +@{ +from collections import OrderedDict +from copy import deepcopy +from rcutils.logging import feature_combinations +from rcutils.logging import get_suffix_from_features +from rcutils.logging import severities +from rcutils.logging import throttle_args +from rcutils.logging import throttle_params + +throttle_args['condition_before'] = 'RCUTILS_LOG_CONDITION_THROTTLE_BEFORE(clock, duration)' +del throttle_params['get_time_point_value'] +throttle_params['clock'] = 'rclcpp::Clock that will be used to get the time point.' +throttle_params.move_to_end('clock', last=False) + +rclcpp_feature_combinations = OrderedDict() +for combinations, feature in feature_combinations.items(): + # skip feature combinations using 'named' + if 'named' in combinations: + continue + rclcpp_feature_combinations[combinations] = feature +# add a stream variant for each available feature combination +stream_arg = 'stream_arg' +for combinations, feature in list(rclcpp_feature_combinations.items()): + combinations = ('stream', ) + combinations + feature = deepcopy(feature) + feature.params[stream_arg] = 'The argument << into a stringstream' + rclcpp_feature_combinations[combinations] = feature + +def get_rclcpp_suffix_from_features(features): + suffix = get_suffix_from_features(features) + if 'stream' in features: + suffix = '_STREAM' + suffix + return suffix +}@ +@[for severity in severities]@ +/** @@name Logging macros for severity @(severity). + */ +///@@{ +#if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_@(severity)) +// empty logging macros for severity @(severity) when being disabled at compile time +@[ for feature_combination in rclcpp_feature_combinations.keys()]@ +@{suffix = get_rclcpp_suffix_from_features(feature_combination)}@ +/// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY. +#define RCLCPP_@(severity)@(suffix)(...) +@[ end for]@ + +#else +@[ for feature_combination in rclcpp_feature_combinations.keys()]@ +@{suffix = get_rclcpp_suffix_from_features(feature_combination)}@ +// The RCLCPP_@(severity)@(suffix) macro is surrounded by do { .. } while (0) +// to implement the standard C macro idiom to make the macro safe in all +// contexts; see http://c-faq.com/cpp/multistmt.html for more information. +/** + * \def RCLCPP_@(severity)@(suffix) + * Log a message with severity @(severity)@ +@[ if rclcpp_feature_combinations[feature_combination].doc_lines]@ + with the following conditions: +@[ else]@ +. +@[ end if]@ +@[ for doc_line in rclcpp_feature_combinations[feature_combination].doc_lines]@ + * @(doc_line) +@[ end for]@ + * \param logger The `rclcpp::Logger` to use +@[ for param_name, doc_line in rclcpp_feature_combinations[feature_combination].params.items()]@ + * \param @(param_name) @(doc_line) +@[ end for]@ +@[ if 'stream' not in feature_combination]@ + * \param ... The format string, followed by the variable arguments for the format string. + * It also accepts a single argument of type std::string. +@[ end if]@ + */ +@{params = rclcpp_feature_combinations[feature_combination].params.keys()}@ +#define RCLCPP_@(severity)@(suffix)(logger@(''.join([', ' + p for p in params]))@ +@[ if 'stream' not in feature_combination]@ +, ...@ +@[ end if]@ +) \ + do { \ + static_assert( \ + ::std::is_same::type>::type, \ + typename ::rclcpp::Logger>::value, \ + "First argument to logging macros must be an rclcpp::Logger"); \ +@[ if 'throttle' in feature_combination]@ \ + auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \ + try { \ + *time_point = c.now().nanoseconds(); \ + } catch (...) { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + "[rclcpp|logging.hpp] RCLCPP_@(severity)@(suffix) could not get current time stamp\n"); \ + return RCUTILS_RET_ERROR; \ + } \ + return RCUTILS_RET_OK; \ + }; \ +@[ end if] \ +@[ if 'stream' in feature_combination]@ + std::stringstream ss; \ + ss << @(stream_arg); \ +@[ end if]@ + RCUTILS_LOG_@(severity)@(get_suffix_from_features(feature_combination))_NAMED( \ +@{params = ['get_time_point' if p == 'clock' and 'throttle' in feature_combination else p for p in params]}@ +@[ if params]@ +@(''.join([' ' + p + ', \\\n' for p in params if p != stream_arg]))@ +@[ end if]@ + logger.get_name(), \ +@[ if 'stream' not in feature_combination]@ + rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \ + RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \ +@[ else]@ + "%s", rclcpp::get_c_string(ss.str())); \ +@[ end if]@ + } while (0) + +@[ end for]@ +#endif +///@@} + +@[end for]@ + +#endif // RCLCPP__LOGGING_HPP_ diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/any_executable.cpp new file mode 100644 index 0000000..e02d107 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/any_executable.cpp @@ -0,0 +1,36 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/any_executable.hpp" + +using rclcpp::executor::AnyExecutable; + +AnyExecutable::AnyExecutable() +: subscription(nullptr), + timer(nullptr), + service(nullptr), + client(nullptr), + callback_group(nullptr), + node_base(nullptr) +{} + +AnyExecutable::~AnyExecutable() +{ + // Make sure that discarded (taken but not executed) AnyExecutable's have + // their callback groups reset. This can happen when an executor is canceled + // between taking an AnyExecutable and executing it. + if (callback_group) { + callback_group->can_be_taken_from().store(true); + } +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/callback_group.cpp new file mode 100644 index 0000000..e9d0e80 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/callback_group.cpp @@ -0,0 +1,116 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/callback_group.hpp" + +#include + +using rclcpp::callback_group::CallbackGroup; +using rclcpp::callback_group::CallbackGroupType; + +CallbackGroup::CallbackGroup(CallbackGroupType group_type) +: type_(group_type), can_be_taken_from_(true) +{} + + +std::atomic_bool & +CallbackGroup::can_be_taken_from() +{ + return can_be_taken_from_; +} + +const CallbackGroupType & +CallbackGroup::type() const +{ + return type_; +} + +void +CallbackGroup::add_subscription( + const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) +{ + std::lock_guard lock(mutex_); + subscription_ptrs_.push_back(subscription_ptr); + subscription_ptrs_.erase( + std::remove_if( + subscription_ptrs_.begin(), + subscription_ptrs_.end(), + [](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}), + subscription_ptrs_.end()); +} + +void +CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr) +{ + std::lock_guard lock(mutex_); + timer_ptrs_.push_back(timer_ptr); + timer_ptrs_.erase( + std::remove_if( + timer_ptrs_.begin(), + timer_ptrs_.end(), + [](rclcpp::TimerBase::WeakPtr x) {return x.expired();}), + timer_ptrs_.end()); +} + +void +CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr) +{ + std::lock_guard lock(mutex_); + service_ptrs_.push_back(service_ptr); + service_ptrs_.erase( + std::remove_if( + service_ptrs_.begin(), + service_ptrs_.end(), + [](rclcpp::ServiceBase::WeakPtr x) {return x.expired();}), + service_ptrs_.end()); +} + +void +CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr) +{ + std::lock_guard lock(mutex_); + client_ptrs_.push_back(client_ptr); + client_ptrs_.erase( + std::remove_if( + client_ptrs_.begin(), + client_ptrs_.end(), + [](rclcpp::ClientBase::WeakPtr x) {return x.expired();}), + client_ptrs_.end()); +} + +void +CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) +{ + std::lock_guard lock(mutex_); + waitable_ptrs_.push_back(waitable_ptr); + waitable_ptrs_.erase( + std::remove_if( + waitable_ptrs_.begin(), + waitable_ptrs_.end(), + [](rclcpp::Waitable::WeakPtr x) {return x.expired();}), + waitable_ptrs_.end()); +} + +void +CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept +{ + std::lock_guard lock(mutex_); + for (auto iter = waitable_ptrs_.begin(); iter != waitable_ptrs_.end(); ++iter) { + const auto shared_ptr = iter->lock(); + if (shared_ptr.get() == waitable_ptr.get()) { + waitable_ptrs_.erase(iter); + break; + } + } +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/client.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/client.cpp new file mode 100644 index 0000000..c2f64e0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/client.cpp @@ -0,0 +1,179 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/client.hpp" + +#include +#include +#include +#include +#include + +#include "rcl/graph.h" +#include "rcl/node.h" +#include "rcl/wait.h" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/logging.hpp" + +using rclcpp::ClientBase; +using rclcpp::exceptions::InvalidNodeError; +using rclcpp::exceptions::throw_from_rcl_error; + +ClientBase::ClientBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph) +: node_graph_(node_graph), + node_handle_(node_base->get_shared_rcl_node_handle()), + context_(node_base->get_context()) +{ + std::weak_ptr weak_node_handle(node_handle_); + rcl_client_t * new_rcl_client = new rcl_client_t; + *new_rcl_client = rcl_get_zero_initialized_client(); + client_handle_.reset( + new_rcl_client, [weak_node_handle](rcl_client_t * client) + { + auto handle = weak_node_handle.lock(); + if (handle) { + if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl client handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Error in destruction of rcl client handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete client; + }); +} + +ClientBase::~ClientBase() +{ + // Make sure the client handle is destructed as early as possible and before the node handle + client_handle_.reset(); +} + +const char * +ClientBase::get_service_name() const +{ + return rcl_client_get_service_name(this->get_client_handle().get()); +} + +std::shared_ptr +ClientBase::get_client_handle() +{ + return client_handle_; +} + +std::shared_ptr +ClientBase::get_client_handle() const +{ + return client_handle_; +} + +bool +ClientBase::service_is_ready() const +{ + bool is_ready; + rcl_ret_t ret = rcl_service_server_is_available( + this->get_rcl_node_handle(), + this->get_client_handle().get(), + &is_ready); + if (RCL_RET_NODE_INVALID == ret) { + const rcl_node_t * node_handle = this->get_rcl_node_handle(); + if (node_handle && !rcl_context_is_valid(node_handle->context)) { + // context is shutdown, do a soft failure + return false; + } + } + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "rcl_service_server_is_available failed"); + } + return is_ready; +} + +bool +ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) +{ + auto start = std::chrono::steady_clock::now(); + // make an event to reuse, rather than create a new one each time + auto node_ptr = node_graph_.lock(); + if (!node_ptr) { + throw InvalidNodeError(); + } + // check to see if the server is ready immediately + if (this->service_is_ready()) { + return true; + } + if (timeout == std::chrono::nanoseconds(0)) { + // check was non-blocking, return immediately + return false; + } + auto event = node_ptr->get_graph_event(); + // update the time even on the first loop to account for time spent in the first call + // to this->server_is_ready() + std::chrono::nanoseconds time_to_wait = + timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); + if (time_to_wait < std::chrono::nanoseconds(0)) { + // Do not allow the time_to_wait to become negative when timeout was originally positive. + // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. + time_to_wait = std::chrono::nanoseconds(0); + } + do { + if (!rclcpp::ok(this->context_)) { + return false; + } + // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation. + // A race condition means that graph changes for services becoming available may trigger the + // wait set to wake up, but then not be reported as ready immediately after the wake up + // (see https://github.com/ros2/rmw_connext/issues/201) + // If no other graph events occur, the wait set will not be triggered again until the timeout + // has been reached, despite the service being available, so we artificially limit the wait + // time to limit the delay. + node_ptr->wait_for_graph_change( + event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100)))); + // Because of the aforementioned race condition, we check if the service is ready even if the + // graph event wasn't triggered. + event->check_and_clear(); + if (this->service_is_ready()) { + return true; + } + // server is not ready, loop if there is time left + if (timeout > std::chrono::nanoseconds(0)) { + time_to_wait = timeout - (std::chrono::steady_clock::now() - start); + } + // if timeout is negative, time_to_wait will never reach zero + } while (time_to_wait > std::chrono::nanoseconds(0)); + return false; // timeout exceeded while waiting for the server to be ready +} + +rcl_node_t * +ClientBase::get_rcl_node_handle() +{ + return node_handle_.get(); +} + +const rcl_node_t * +ClientBase::get_rcl_node_handle() const +{ + return node_handle_.get(); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/clock.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/clock.cpp new file mode 100644 index 0000000..f692843 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/clock.cpp @@ -0,0 +1,144 @@ +// Copyright 2017 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 "rclcpp/clock.hpp" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/logging_macros.h" + +namespace rclcpp +{ + +JumpHandler::JumpHandler( + pre_callback_t pre_callback, + post_callback_t post_callback, + const rcl_jump_threshold_t & threshold) +: pre_callback(pre_callback), + post_callback(post_callback), + notice_threshold(threshold) +{} + +Clock::Clock(rcl_clock_type_t clock_type) +{ + allocator_ = rcl_get_default_allocator(); + auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_); + if (ret != RCL_RET_OK) { + exceptions::throw_from_rcl_error(ret, "failed to initialize rcl clock"); + } +} + +Clock::~Clock() +{ + auto ret = rcl_clock_fini(&rcl_clock_); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR("Failed to fini rcl clock."); + } +} + +Time +Clock::now() +{ + Time now(0, 0, rcl_clock_.type); + + auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds); + if (ret != RCL_RET_OK) { + exceptions::throw_from_rcl_error(ret, "could not get current time stamp"); + } + + return now; +} + +bool +Clock::ros_time_is_active() +{ + if (!rcl_clock_valid(&rcl_clock_)) { + RCUTILS_LOG_ERROR("ROS time not valid!"); + return false; + } + + bool is_enabled = false; + auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled); + if (ret != RCL_RET_OK) { + exceptions::throw_from_rcl_error( + ret, "Failed to check ros_time_override_status"); + } + return is_enabled; +} + +rcl_clock_t * +Clock::get_clock_handle() noexcept +{ + return &rcl_clock_; +} + +rcl_clock_type_t +Clock::get_clock_type() const noexcept +{ + return rcl_clock_.type; +} + +void +Clock::on_time_jump( + const struct rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data) +{ + const auto * handler = static_cast(user_data); + if (nullptr == handler) { + return; + } + if (before_jump && handler->pre_callback) { + handler->pre_callback(); + } else if (!before_jump && handler->post_callback) { + handler->post_callback(*time_jump); + } +} + +JumpHandler::SharedPtr +Clock::create_jump_callback( + JumpHandler::pre_callback_t pre_callback, + JumpHandler::post_callback_t post_callback, + const rcl_jump_threshold_t & threshold) +{ + // Allocate a new jump handler + JumpHandler::UniquePtr handler(new JumpHandler(pre_callback, post_callback, threshold)); + if (nullptr == handler) { + throw std::bad_alloc{}; + } + + // Try to add the jump callback to the clock + rcl_ret_t ret = rcl_clock_add_jump_callback( + &rcl_clock_, threshold, Clock::on_time_jump, + handler.get()); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback"); + } + + // *INDENT-OFF* + // create shared_ptr that removes the callback automatically when all copies are destructed + // TODO(dorezyuk) UB, if the clock leaves scope before the JumpHandler + return JumpHandler::SharedPtr(handler.release(), [this](JumpHandler * handler) noexcept { + rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, Clock::on_time_jump, + handler); + delete handler; + handler = NULL; + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR("Failed to remove time jump callback"); + } + }); + // *INDENT-ON* +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/context.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/context.cpp new file mode 100644 index 0000000..a93086a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/context.cpp @@ -0,0 +1,309 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/context.hpp" + +#include +#include +#include +#include + +#include "rcl/init.h" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rmw/impl/cpp/demangle.hpp" + +/// Mutex to protect initialized contexts. +static std::mutex g_contexts_mutex; +/// Weak list of context to be shutdown by the signal handler. +static std::vector> g_contexts; + +using rclcpp::Context; + +Context::Context() +: rcl_context_(nullptr), shutdown_reason_("") {} + +Context::~Context() +{ + // acquire the init lock to prevent race conditions with init and shutdown + // this will not prevent errors, but will maybe make them easier to reproduce + std::lock_guard lock(init_mutex_); + try { + this->shutdown("context destructor was called while still not shutdown"); + // at this point it is shutdown and cannot reinit + // clean_up will finalize the rcl context + this->clean_up(); + } catch (const std::exception & exc) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context(): %s", exc.what()); + } catch (...) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context()"); + } +} + +RCLCPP_LOCAL +void +__delete_context(rcl_context_t * context) +{ + if (context) { + if (rcl_context_is_valid(context)) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "rcl context unexpectedly not shutdown during cleanup"); + } else { + // if context pointer is not null and is shutdown, then it's ready for fini + rcl_ret_t ret = rcl_context_fini(context); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize context: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } + delete context; + } +} + +void +Context::init( + int argc, + char const * const argv[], + const rclcpp::InitOptions & init_options) +{ + std::lock_guard init_lock(init_mutex_); + if (this->is_valid()) { + throw rclcpp::ContextAlreadyInitialized(); + } + this->clean_up(); + rcl_context_.reset(new rcl_context_t, __delete_context); + *rcl_context_.get() = rcl_get_zero_initialized_context(); + rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get()); + if (RCL_RET_OK != ret) { + rcl_context_.reset(); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl"); + } + init_options_ = init_options; + + std::lock_guard lock(g_contexts_mutex); + g_contexts.push_back(this->shared_from_this()); +} + +bool +Context::is_valid() const +{ + // Take a local copy of the shared pointer to avoid it getting nulled under our feet. + auto local_rcl_context = rcl_context_; + if (!local_rcl_context) { + return false; + } + return rcl_context_is_valid(local_rcl_context.get()); +} + +const rclcpp::InitOptions & +Context::get_init_options() const +{ + return init_options_; +} + +rclcpp::InitOptions +Context::get_init_options() +{ + return init_options_; +} + +std::string +Context::shutdown_reason() +{ + std::lock_guard lock(init_mutex_); + return shutdown_reason_; +} + +bool +Context::shutdown(const std::string & reason) +{ + // prevent races + std::lock_guard init_lock(init_mutex_); + // ensure validity + if (!this->is_valid()) { + // if it is not valid, then it cannot be shutdown + return false; + } + // rcl shutdown + rcl_ret_t ret = rcl_shutdown(rcl_context_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + // set shutdown reason + shutdown_reason_ = reason; + // call each shutdown callback + for (const auto & callback : on_shutdown_callbacks_) { + callback(); + } + // interrupt all blocking sleep_for() and all blocking executors or wait sets + this->interrupt_all_sleep_for(); + this->interrupt_all_wait_sets(); + // remove self from the global contexts + std::lock_guard context_lock(g_contexts_mutex); + for (auto it = g_contexts.begin(); it != g_contexts.end(); ) { + auto shared_context = it->lock(); + if (shared_context.get() == this) { + it = g_contexts.erase(it); + break; + } else { + ++it; + } + } + return true; +} + +rclcpp::Context::OnShutdownCallback +Context::on_shutdown(OnShutdownCallback callback) +{ + on_shutdown_callbacks_.push_back(callback); + return callback; +} + +const std::vector & +Context::get_on_shutdown_callbacks() const +{ + return on_shutdown_callbacks_; +} + +std::vector & +Context::get_on_shutdown_callbacks() +{ + return on_shutdown_callbacks_; +} + +std::shared_ptr +Context::get_rcl_context() +{ + return rcl_context_; +} + +bool +Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) +{ + std::chrono::nanoseconds time_left = nanoseconds; + { + std::unique_lock lock(interrupt_mutex_); + auto start = std::chrono::steady_clock::now(); + // this will release the lock while waiting + interrupt_condition_variable_.wait_for(lock, nanoseconds); + time_left -= std::chrono::steady_clock::now() - start; + } + if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) { + return sleep_for(time_left); + } + // Return true if the timeout elapsed successfully, otherwise false. + return this->is_valid(); +} + +void +Context::interrupt_all_sleep_for() +{ + interrupt_condition_variable_.notify_all(); +} + +rcl_guard_condition_t * +Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(interrupt_guard_cond_handles_mutex_); + auto kv = interrupt_guard_cond_handles_.find(wait_set); + if (kv != interrupt_guard_cond_handles_.end()) { + return &kv->second; + } else { + rcl_guard_condition_t handle = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); + auto ret = rcl_guard_condition_init(&handle, this->get_rcl_context().get(), options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition"); + } + interrupt_guard_cond_handles_.emplace(wait_set, handle); + return &interrupt_guard_cond_handles_[wait_set]; + } +} + +void +Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(interrupt_guard_cond_handles_mutex_); + auto kv = interrupt_guard_cond_handles_.find(wait_set); + if (kv != interrupt_guard_cond_handles_.end()) { + rcl_ret_t ret = rcl_guard_condition_fini(&kv->second); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to destroy sigint guard condition"); + } + interrupt_guard_cond_handles_.erase(kv); + } else { + throw std::runtime_error("Tried to release sigint guard condition for nonexistent wait set"); + } +} + +void +Context::release_interrupt_guard_condition( + rcl_wait_set_t * wait_set, + const std::nothrow_t &) noexcept +{ + try { + this->release_interrupt_guard_condition(wait_set); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught %s exception when releasing interrupt guard condition: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught unknown exception when releasing interrupt guard condition"); + } +} + +void +Context::interrupt_all_wait_sets() +{ + std::lock_guard lock(interrupt_guard_cond_handles_mutex_); + for (auto & kv : interrupt_guard_cond_handles_) { + rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second)); + if (status != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to trigger guard condition in Context::interrupt_all_wait_sets(): %s", + rcl_get_error_string().str); + } + } +} + +void +Context::clean_up() +{ + shutdown_reason_ = ""; + rcl_context_.reset(); +} + +std::vector +rclcpp::get_contexts() +{ + std::lock_guard lock(g_contexts_mutex); + std::vector shared_contexts; + for (auto it = g_contexts.begin(); it != g_contexts.end(); /* noop */) { + auto context_ptr = it->lock(); + if (!context_ptr) { + // remove invalid weak context pointers + it = g_contexts.erase(it); + } else { + ++it; + shared_contexts.push_back(context_ptr); + } + } + return shared_contexts; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/contexts/default_context.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/contexts/default_context.cpp new file mode 100644 index 0000000..53c6c9f --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/contexts/default_context.cpp @@ -0,0 +1,27 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/contexts/default_context.hpp" + +using rclcpp::contexts::default_context::DefaultContext; + +DefaultContext::DefaultContext() +{} + +DefaultContext::SharedPtr +rclcpp::contexts::default_context::get_global_default_context() +{ + static DefaultContext::SharedPtr default_context = DefaultContext::make_shared(); + return default_context; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp new file mode 100644 index 0000000..be6ec91 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp @@ -0,0 +1,35 @@ +// Copyright 2019 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 + +namespace rclcpp +{ +namespace detail +{ + +bool +RMWImplementationSpecificPayload::has_been_customized() const +{ + return nullptr != this->get_implementation_identifier(); +} + +const char * +RMWImplementationSpecificPayload::get_implementation_identifier() const +{ + return nullptr; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp new file mode 100644 index 0000000..cd7580d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp @@ -0,0 +1,33 @@ +// Copyright 2019 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/publisher.h" + +namespace rclcpp +{ +namespace detail +{ + +void +RMWImplementationSpecificPublisherPayload::modify_rmw_publisher_options( + rmw_publisher_options_t & rmw_publisher_options) const +{ + // By default, do not mutate the rmw publisher options. + (void)rmw_publisher_options; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp new file mode 100644 index 0000000..c84c3d4 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp @@ -0,0 +1,33 @@ +// Copyright 2019 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/subscription.h" + +namespace rclcpp +{ +namespace detail +{ + +void +RMWImplementationSpecificSubscriptionPayload::modify_rmw_subscription_options( + rmw_subscription_options_t & rmw_subscription_options) const +{ + // By default, do not mutate the rmw subscription options. + (void)rmw_subscription_options; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/duration.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/duration.cpp new file mode 100644 index 0000000..35db016 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/duration.cpp @@ -0,0 +1,242 @@ +// Copyright 2017 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 +#include +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/time.hpp" + +#include "builtin_interfaces/msg/duration.hpp" + +#include "rcl/time.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/logging_macros.h" + +namespace rclcpp +{ + +Duration::Duration(int32_t seconds, uint32_t nanoseconds) +{ + rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(seconds)); + rcl_duration_.nanoseconds += nanoseconds; +} + +Duration::Duration(int64_t nanoseconds) +{ + rcl_duration_.nanoseconds = nanoseconds; +} + +Duration::Duration(std::chrono::nanoseconds nanoseconds) +{ + rcl_duration_.nanoseconds = nanoseconds.count(); +} + +Duration::Duration(const Duration & rhs) +{ + rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; +} + +Duration::Duration( + const builtin_interfaces::msg::Duration & duration_msg) +{ + rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += duration_msg.nanosec; +} + +Duration::Duration(const rcl_duration_t & duration) +: rcl_duration_(duration) +{ + // noop +} + +Duration::operator builtin_interfaces::msg::Duration() const +{ + builtin_interfaces::msg::Duration msg_duration; + msg_duration.sec = static_cast(RCL_NS_TO_S(rcl_duration_.nanoseconds)); + msg_duration.nanosec = + static_cast(rcl_duration_.nanoseconds % (1000 * 1000 * 1000)); + return msg_duration; +} + +Duration & +Duration::operator=(const Duration & rhs) +{ + rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; + return *this; +} + +Duration & +Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg) +{ + rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += duration_msg.nanosec; + return *this; +} + +bool +Duration::operator==(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator<(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds < rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator<=(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds <= rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator>=(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds >= rhs.rcl_duration_.nanoseconds; +} + +bool +Duration::operator>(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds > rhs.rcl_duration_.nanoseconds; +} + +void +bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max) +{ + auto abs_lhs = (uint64_t)std::abs(lhsns); + auto abs_rhs = (uint64_t)std::abs(rhsns); + + if (lhsns > 0 && rhsns > 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + } else if (lhsns < 0 && rhsns < 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + } +} + +Duration +Duration::operator+(const rclcpp::Duration & rhs) const +{ + bounds_check_duration_sum( + this->rcl_duration_.nanoseconds, + rhs.rcl_duration_.nanoseconds, + std::numeric_limits::max()); + return Duration( + rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds); +} + +void +bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max) +{ + auto abs_lhs = (uint64_t)std::abs(lhsns); + auto abs_rhs = (uint64_t)std::abs(rhsns); + + if (lhsns > 0 && rhsns < 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::overflow_error("duration subtraction leads to int64_t overflow"); + } + } else if (lhsns < 0 && rhsns > 0) { + if (abs_lhs + abs_rhs > (uint64_t) max) { + throw std::underflow_error("duration subtraction leads to int64_t underflow"); + } + } +} + +Duration +Duration::operator-(const rclcpp::Duration & rhs) const +{ + bounds_check_duration_difference( + this->rcl_duration_.nanoseconds, + rhs.rcl_duration_.nanoseconds, + std::numeric_limits::max()); + + return Duration( + rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); +} + +void +bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) +{ + auto abs_dns = static_cast(std::abs(dns)); + auto abs_scale = std::abs(scale); + + if (abs_scale > 1.0 && abs_dns > static_cast(max / abs_scale)) { + if ((dns > 0 && scale > 0) || (dns < 0 && scale < 0)) { + throw std::overflow_error("duration scaling leads to int64_t overflow"); + } else { + throw std::underflow_error("duration scaling leads to int64_t underflow"); + } + } +} + +Duration +Duration::operator*(double scale) const +{ + if (!std::isfinite(scale)) { + throw std::runtime_error("abnormal scale in rclcpp::Duration"); + } + bounds_check_duration_scale( + this->rcl_duration_.nanoseconds, + scale, + std::numeric_limits::max()); + return Duration(static_cast(rcl_duration_.nanoseconds * scale)); +} + +rcl_duration_value_t +Duration::nanoseconds() const +{ + return rcl_duration_.nanoseconds; +} + +Duration +Duration::max() +{ + return Duration(std::numeric_limits::max(), 999999999); +} + +double +Duration::seconds() const +{ + return std::chrono::duration(std::chrono::nanoseconds(rcl_duration_.nanoseconds)).count(); +} + +rmw_time_t +Duration::to_rmw_time() const +{ + // reuse conversion logic from msg creation + builtin_interfaces::msg::Duration msg = *this; + rmw_time_t result; + result.sec = msg.sec; + result.nsec = msg.nanosec; + return result; +} + +Duration +Duration::from_seconds(double seconds) +{ + return Duration(static_cast(RCL_S_TO_NS(seconds))); +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/event.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/event.cpp new file mode 100644 index 0000000..6be20a6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/event.cpp @@ -0,0 +1,41 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/event.hpp" + +namespace rclcpp +{ + +Event::Event() +: state_(false) {} + +bool +Event::set() +{ + return state_.exchange(true); +} + +bool +Event::check() +{ + return state_.load(); +} + +bool +Event::check_and_clear() +{ + return state_.exchange(false); +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/exceptions.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/exceptions.cpp new file mode 100644 index 0000000..4371066 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/exceptions.cpp @@ -0,0 +1,146 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/exceptions.hpp" + +#include +#include +#include +#include + +using namespace std::string_literals; + +namespace rclcpp +{ +namespace exceptions +{ + +std::string +NameValidationError::format_error( + const char * name_type, + const char * name, + const char * error_msg, + size_t invalid_index) +{ + std::string msg = ""; + msg += "Invalid "s + name_type + ": " + error_msg + ":\n"; + msg += " '"s + name + "'\n"; + msg += " "s + std::string(invalid_index, ' ') + "^\n"; + return msg; +} + +std::exception_ptr +from_rcl_error( + rcl_ret_t ret, + const std::string & prefix, + const rcl_error_state_t * error_state, + void (* reset_error)()) +{ + if (RCL_RET_OK == ret) { + throw std::invalid_argument("ret is RCL_RET_OK"); + } + if (!error_state) { + error_state = rcl_get_error_state(); + } + if (!error_state) { + throw std::runtime_error("rcl error state is not set"); + } + std::string formatted_prefix = prefix; + if (!prefix.empty()) { + formatted_prefix += ": "; + } + RCLErrorBase base_exc(ret, error_state); + if (reset_error) { + reset_error(); + } + switch (ret) { + case RCL_RET_BAD_ALLOC: + return std::make_exception_ptr(RCLBadAlloc(base_exc)); + case RCL_RET_INVALID_ARGUMENT: + return std::make_exception_ptr(RCLInvalidArgument(base_exc, formatted_prefix)); + case RCL_RET_INVALID_ROS_ARGS: + return std::make_exception_ptr(RCLInvalidROSArgsError(base_exc, formatted_prefix)); + default: + return std::make_exception_ptr(RCLError(base_exc, formatted_prefix)); + } +} + +void +throw_from_rcl_error( + rcl_ret_t ret, + const std::string & prefix, + const rcl_error_state_t * error_state, + void (* reset_error)()) +{ + // We expect this to either throw a standard error, + // or to generate an error pointer (which is caught + // in err, and immediately thrown) + auto err = from_rcl_error(ret, prefix, error_state, reset_error); + std::rethrow_exception(err); +} + +RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state) +: ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number), + formatted_message(rcl_get_error_string().str) +{} + +RCLError::RCLError( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix) +: RCLError(RCLErrorBase(ret, error_state), prefix) +{} + +RCLError::RCLError( + const RCLErrorBase & base_exc, + const std::string & prefix) +: RCLErrorBase(base_exc), std::runtime_error(prefix + base_exc.formatted_message) +{} + +RCLBadAlloc::RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state) +: RCLBadAlloc(RCLErrorBase(ret, error_state)) +{} + +RCLBadAlloc::RCLBadAlloc(const RCLErrorBase & base_exc) +: RCLErrorBase(base_exc), std::bad_alloc() +{} + +RCLInvalidArgument::RCLInvalidArgument( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix) +: RCLInvalidArgument(RCLErrorBase(ret, error_state), prefix) +{} + +RCLInvalidArgument::RCLInvalidArgument( + const RCLErrorBase & base_exc, + const std::string & prefix) +: RCLErrorBase(base_exc), std::invalid_argument(prefix + base_exc.formatted_message) +{} + +RCLInvalidROSArgsError::RCLInvalidROSArgsError( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix) +: RCLInvalidROSArgsError(RCLErrorBase(ret, error_state), prefix) +{} + +RCLInvalidROSArgsError::RCLInvalidROSArgsError( + const RCLErrorBase & base_exc, + const std::string & prefix) +: RCLErrorBase(base_exc), std::runtime_error(prefix + base_exc.formatted_message) +{} + +} // namespace exceptions +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/executor.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/executor.cpp new file mode 100644 index 0000000..1209b2a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/executor.cpp @@ -0,0 +1,631 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/scope_exit.hpp" +#include "rclcpp/utilities.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +#include "rcutils/logging_macros.h" + +using rclcpp::exceptions::throw_from_rcl_error; +using rclcpp::executor::AnyExecutable; +using rclcpp::executor::Executor; +using rclcpp::executor::ExecutorArgs; +using rclcpp::executor::FutureReturnCode; + +Executor::Executor(const ExecutorArgs & args) +: spinning(false), + memory_strategy_(args.memory_strategy) +{ + rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); + rcl_ret_t ret = rcl_guard_condition_init( + &interrupt_guard_condition_, args.context->get_rcl_context().get(), guard_condition_options); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor"); + } + + // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, + // and one for the executor's guard cond (interrupt_guard_condition_) + + // Put the global ctrl-c guard condition in + memory_strategy_->add_guard_condition(args.context->get_interrupt_guard_condition(&wait_set_)); + + // Put the executor's guard condition in + memory_strategy_->add_guard_condition(&interrupt_guard_condition_); + rcl_allocator_t allocator = memory_strategy_->get_allocator(); + + // Store the context for later use. + context_ = args.context; + + ret = rcl_wait_set_init( + &wait_set_, + 0, 2, 0, 0, 0, 0, + context_->get_rcl_context().get(), + allocator); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to create wait set: %s", rcl_get_error_string().str); + rcl_reset_error(); + if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + throw std::runtime_error("Failed to create wait set in Executor constructor"); + } +} + +Executor::~Executor() +{ + // Disassocate all nodes + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (node) { + std::atomic_bool & has_executor = node->get_associated_with_executor_atomic(); + has_executor.store(false); + } + } + weak_nodes_.clear(); + for (auto & guard_condition : guard_conditions_) { + memory_strategy_->remove_guard_condition(guard_condition); + } + guard_conditions_.clear(); + + // Finalize the wait set. + if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy wait set: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + // Remove and release the sigint guard condition + memory_strategy_->remove_guard_condition(context_->get_interrupt_guard_condition(&wait_set_)); + context_->release_interrupt_guard_condition(&wait_set_, std::nothrow); +} + +void +Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +{ + // If the node already has an executor + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error("Node has already been added to an executor."); + } + // Check to ensure node not already added + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (node == node_ptr) { + // TODO(jacquelinekay): Use a different error here? + throw std::runtime_error("Cannot add node to executor, node already added."); + } + } + weak_nodes_.push_back(node_ptr); + guard_conditions_.push_back(node_ptr->get_notify_guard_condition()); + if (notify) { + // Interrupt waiting to handle new node + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + throw std::runtime_error(rcl_get_error_string().str); + } + } + // Add the node's notify condition to the guard condition handles + std::unique_lock lock(memory_strategy_mutex_); + memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition()); +} + +void +Executor::add_node(std::shared_ptr node_ptr, bool notify) +{ + this->add_node(node_ptr->get_node_base_interface(), notify); +} + +void +Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +{ + bool node_removed = false; + { + auto node_it = weak_nodes_.begin(); + auto gc_it = guard_conditions_.begin(); + while (node_it != weak_nodes_.end()) { + bool matched = (node_it->lock() == node_ptr); + if (matched) { + node_it = weak_nodes_.erase(node_it); + gc_it = guard_conditions_.erase(gc_it); + node_removed = true; + } else { + ++node_it; + ++gc_it; + } + } + } + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + if (notify) { + // If the node was matched and removed, interrupt waiting + if (node_removed) { + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + throw std::runtime_error(rcl_get_error_string().str); + } + } + } + std::unique_lock lock(memory_strategy_mutex_); + memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); +} + +void +Executor::remove_node(std::shared_ptr node_ptr, bool notify) +{ + this->remove_node(node_ptr->get_node_base_interface(), notify); +} + +void +Executor::spin_node_once_nanoseconds( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds timeout) +{ + this->add_node(node, false); + // non-blocking = true + spin_once(timeout); + this->remove_node(node, false); +} + +void +Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) +{ + this->add_node(node, false); + spin_some(); + this->remove_node(node, false); +} + +void +Executor::spin_node_some(std::shared_ptr node) +{ + this->spin_node_some(node->get_node_base_interface()); +} + +void +Executor::spin_some(std::chrono::nanoseconds max_duration) +{ + auto start = std::chrono::steady_clock::now(); + auto max_duration_not_elapsed = [max_duration, start]() { + if (std::chrono::nanoseconds(0) == max_duration) { + // told to spin forever if need be + return true; + } else if (std::chrono::steady_clock::now() - start < max_duration) { + // told to spin only for some maximum amount of time + return true; + } + // spun too long + return false; + }; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { + AnyExecutable any_exec; + if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) { + execute_any_executable(any_exec); + } else { + break; + } + } +} + +void +Executor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + AnyExecutable any_exec; + if (get_next_executable(any_exec, timeout)) { + execute_any_executable(any_exec); + } +} + +void +Executor::spin_once(std::chrono::nanoseconds timeout) +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin_once() called while already spinning"); + } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + spin_once_impl(timeout); +} + +void +Executor::cancel() +{ + spinning.store(false); + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + throw std::runtime_error(rcl_get_error_string().str); + } +} + +void +Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) +{ + if (memory_strategy == nullptr) { + throw std::runtime_error("Received NULL memory strategy in executor."); + } + memory_strategy_ = memory_strategy; +} + +void +Executor::execute_any_executable(AnyExecutable & any_exec) +{ + if (!spinning.load()) { + return; + } + if (any_exec.timer) { + execute_timer(any_exec.timer); + } + if (any_exec.subscription) { + execute_subscription(any_exec.subscription); + } + if (any_exec.service) { + execute_service(any_exec.service); + } + if (any_exec.client) { + execute_client(any_exec.client); + } + if (any_exec.waitable) { + any_exec.waitable->execute(); + } + // Reset the callback_group, regardless of type + any_exec.callback_group->can_be_taken_from().store(true); + // Wake the wait, because it may need to be recalculated or work that + // was previously blocked is now available. + if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { + throw std::runtime_error(rcl_get_error_string().str); + } +} + +void +Executor::execute_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rmw_message_info_t message_info; + message_info.from_intra_process = false; + + if (subscription->is_serialized()) { + auto serialized_msg = subscription->create_serialized_message(); + auto ret = rcl_take_serialized_message( + subscription->get_subscription_handle().get(), + serialized_msg.get(), &message_info, nullptr); + if (RCL_RET_OK == ret) { + auto void_serialized_msg = std::static_pointer_cast(serialized_msg); + subscription->handle_message(void_serialized_msg, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take_serialized failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + subscription->return_serialized_message(serialized_msg); + } else if (subscription->can_loan_messages()) { + void * loaned_msg = nullptr; + auto ret = rcl_take_loaned_message( + subscription->get_subscription_handle().get(), + &loaned_msg, + &message_info, + nullptr); + if (RCL_RET_OK == ret) { + subscription->handle_loaned_message(loaned_msg, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take_loaned failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + ret = rcl_return_loaned_message_from_subscription( + subscription->get_subscription_handle().get(), + loaned_msg); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "return_loaned_message failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + } + loaned_msg = nullptr; + } else { + std::shared_ptr message = subscription->create_message(); + auto ret = rcl_take( + subscription->get_subscription_handle().get(), + message.get(), &message_info, nullptr); + if (RCL_RET_OK == ret) { + subscription->handle_message(message, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "could not deserialize serialized message on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + subscription->return_message(message); + } +} + +void +Executor::execute_timer( + rclcpp::TimerBase::SharedPtr timer) +{ + timer->execute_callback(); +} + +void +Executor::execute_service( + rclcpp::ServiceBase::SharedPtr service) +{ + auto request_header = service->create_request_header(); + std::shared_ptr request = service->create_request(); + rcl_ret_t status = rcl_take_request( + service->get_service_handle().get(), + request_header.get(), + request.get()); + if (status == RCL_RET_OK) { + service->handle_request(request_header, request); + } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take request failed for server of service '%s': %s", + service->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } +} + +void +Executor::execute_client( + rclcpp::ClientBase::SharedPtr client) +{ + auto request_header = client->create_request_header(); + std::shared_ptr response = client->create_response(); + rcl_ret_t status = rcl_take_response( + client->get_client_handle().get(), + request_header.get(), + response.get()); + if (status == RCL_RET_OK) { + client->handle_response(request_header, response); + } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take response failed for client of service '%s': %s", + client->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } +} + +void +Executor::wait_for_work(std::chrono::nanoseconds timeout) +{ + { + std::unique_lock lock(memory_strategy_mutex_); + + // Collect the subscriptions and timers to be waited on + memory_strategy_->clear_handles(); + bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); + + // Clean up any invalid nodes, if they were detected + if (has_invalid_weak_nodes) { + auto node_it = weak_nodes_.begin(); + auto gc_it = guard_conditions_.begin(); + while (node_it != weak_nodes_.end()) { + if (node_it->expired()) { + node_it = weak_nodes_.erase(node_it); + memory_strategy_->remove_guard_condition(*gc_it); + gc_it = guard_conditions_.erase(gc_it); + } else { + ++node_it; + ++gc_it; + } + } + } + // clear wait set + if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) { + throw std::runtime_error("Couldn't clear wait set"); + } + + // The size of waitables are accounted for in size of the other entities + rcl_ret_t ret = rcl_wait_set_resize( + &wait_set_, memory_strategy_->number_of_ready_subscriptions(), + memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), + memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), + memory_strategy_->number_of_ready_events()); + if (RCL_RET_OK != ret) { + throw std::runtime_error( + std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str); + } + + if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { + throw std::runtime_error("Couldn't fill wait set"); + } + } + rcl_ret_t status = + rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); + if (status == RCL_RET_WAIT_SET_EMPTY) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in rcl_wait(). This should never happen."); + } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(status, "rcl_wait() failed"); + } + + // check the null handles in the wait set and remove them from the handles in memory strategy + // for callback-based entities + memory_strategy_->remove_null_handles(&wait_set_); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (!group) { + return nullptr; + } + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto callback_group = weak_group.lock(); + if (callback_group == group) { + return node; + } + } + } + return nullptr; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) +{ + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto timer_ref = group->find_timer_ptrs_if( + [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { + return timer_ptr == timer; + }); + if (timer_ref) { + return group; + } + } + } + return rclcpp::callback_group::CallbackGroup::SharedPtr(); +} + +bool +Executor::get_next_ready_executable(AnyExecutable & any_executable) +{ + // Check the timers to see if there are any that are ready + memory_strategy_->get_next_timer(any_executable, weak_nodes_); + if (any_executable.timer) { + return true; + } + // Check the subscriptions to see if there are any that are ready + memory_strategy_->get_next_subscription(any_executable, weak_nodes_); + if (any_executable.subscription) { + return true; + } + // Check the services to see if there are any that are ready + memory_strategy_->get_next_service(any_executable, weak_nodes_); + if (any_executable.service) { + return true; + } + // Check the clients to see if there are any that are ready + memory_strategy_->get_next_client(any_executable, weak_nodes_); + if (any_executable.client) { + return true; + } + // Check the waitables to see if there are any that are ready + memory_strategy_->get_next_waitable(any_executable, weak_nodes_); + if (any_executable.waitable) { + return true; + } + // If there is no ready executable, return a null ptr + return false; +} + +bool +Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout) +{ + bool success = false; + // Check to see if there are any subscriptions or timers needing service + // TODO(wjwwood): improve run to run efficiency of this function + success = get_next_ready_executable(any_executable); + // If there are none + if (!success) { + // Wait for subscriptions or timers to work on + wait_for_work(timeout); + if (!spinning.load()) { + return false; + } + // Try again + success = get_next_ready_executable(any_executable); + } + // At this point any_exec should be valid with either a valid subscription + // or a valid timer, or it should be a null shared_ptr + if (success) { + // If it is valid, check to see if the group is mutually exclusive or + // not, then mark it accordingly + using callback_group::CallbackGroupType; + if ( + any_executable.callback_group && + any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) + { + // It should not have been taken otherwise + assert(any_executable.callback_group->can_be_taken_from().load()); + // Set to false to indicate something is being run from this group + // This is reset to true either when the any_exec is executed or when the + // any_exec is destructued + any_executable.callback_group->can_be_taken_from().store(false); + } + } + return success; +} + +std::ostream & +rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_return_code) +{ + return os << to_string(future_return_code); +} + +std::string +rclcpp::executor::to_string(const FutureReturnCode & future_return_code) +{ + using enum_type = std::underlying_type::type; + std::string prefix = "Unknown enum value ("; + std::string ret_as_string = std::to_string(static_cast(future_return_code)); + switch (future_return_code) { + case FutureReturnCode::SUCCESS: + prefix = "SUCCESS ("; + break; + case FutureReturnCode::INTERRUPTED: + prefix = "INTERRUPTED ("; + break; + case FutureReturnCode::TIMEOUT: + prefix = "TIMEOUT ("; + break; + } + return prefix + ret_as_string + ")"; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/executors.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/executors.cpp new file mode 100644 index 0000000..0a900c0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/executors.cpp @@ -0,0 +1,43 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/executors.hpp" + +void +rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.spin_node_some(node_ptr); +} + +void +rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr) +{ + rclcpp::spin_some(node_ptr->get_node_base_interface()); +} + +void +rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node_ptr); + exec.spin(); + exec.remove_node(node_ptr); +} + +void +rclcpp::spin(rclcpp::Node::SharedPtr node_ptr) +{ + rclcpp::spin(node_ptr->get_node_base_interface()); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp new file mode 100644 index 0000000..e2f878a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -0,0 +1,116 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/executors/multi_threaded_executor.hpp" + +#include +#include +#include +#include + +#include "rclcpp/utilities.hpp" +#include "rclcpp/scope_exit.hpp" + +using rclcpp::executors::MultiThreadedExecutor; + +MultiThreadedExecutor::MultiThreadedExecutor( + const rclcpp::executor::ExecutorArgs & args, + size_t number_of_threads, + bool yield_before_execute, + std::chrono::nanoseconds next_exec_timeout) +: executor::Executor(args), + yield_before_execute_(yield_before_execute), + next_exec_timeout_(next_exec_timeout) +{ + number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency(); + if (number_of_threads_ == 0) { + number_of_threads_ = 1; + } +} + +MultiThreadedExecutor::~MultiThreadedExecutor() {} + +void +MultiThreadedExecutor::spin() +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin() called while already spinning"); + } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + std::vector threads; + size_t thread_id = 0; + { + std::lock_guard wait_lock(wait_mutex_); + for (; thread_id < number_of_threads_ - 1; ++thread_id) { + auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id); + threads.emplace_back(func); + } + } + + run(thread_id); + for (auto & thread : threads) { + thread.join(); + } +} + +size_t +MultiThreadedExecutor::get_number_of_threads() +{ + return number_of_threads_; +} + +void +MultiThreadedExecutor::run(size_t) +{ + while (rclcpp::ok(this->context_) && spinning.load()) { + executor::AnyExecutable any_exec; + { + std::lock_guard wait_lock(wait_mutex_); + if (!rclcpp::ok(this->context_) || !spinning.load()) { + return; + } + if (!get_next_executable(any_exec, next_exec_timeout_)) { + continue; + } + if (any_exec.timer) { + // Guard against multiple threads getting the same timer. + if (scheduled_timers_.count(any_exec.timer) != 0) { + // Make sure that any_exec's callback group is reset before + // the lock is released. + if (any_exec.callback_group) { + any_exec.callback_group->can_be_taken_from().store(true); + } + continue; + } + scheduled_timers_.insert(any_exec.timer); + } + } + if (yield_before_execute_) { + std::this_thread::yield(); + } + + execute_any_executable(any_exec); + + if (any_exec.timer) { + std::lock_guard wait_lock(wait_mutex_); + auto it = scheduled_timers_.find(any_exec.timer); + if (it != scheduled_timers_.end()) { + scheduled_timers_.erase(it); + } + } + // Clear the callback_group to prevent the AnyExecutable destructor from + // resetting the callback group `can_be_taken_from` + any_exec.callback_group.reset(); + } +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp new file mode 100644 index 0000000..7f699d8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -0,0 +1,39 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/any_executable.hpp" +#include "rclcpp/scope_exit.hpp" + +using rclcpp::executors::SingleThreadedExecutor; + +SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executor::ExecutorArgs & args) +: executor::Executor(args) {} + +SingleThreadedExecutor::~SingleThreadedExecutor() {} + +void +SingleThreadedExecutor::spin() +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin() called while already spinning"); + } + RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { + rclcpp::executor::AnyExecutable any_executable; + if (get_next_executable(any_executable)) { + execute_any_executable(any_executable); + } + } +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp new file mode 100644 index 0000000..b6976e2 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp @@ -0,0 +1,203 @@ +// Copyright 2017 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 "rclcpp/expand_topic_or_service_name.hpp" + +#include + +#include "rcl/expand_topic_name.h" +#include "rcl/validate_topic_name.h" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/scope_exit.hpp" +#include "rcutils/logging_macros.h" +#include "rcutils/types/string_map.h" +#include "rmw/error_handling.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" +#include "rmw/validate_full_topic_name.h" + +using rclcpp::exceptions::throw_from_rcl_error; + +std::string +rclcpp::expand_topic_or_service_name( + const std::string & name, + const std::string & node_name, + const std::string & namespace_, + bool is_service) +{ + char * expanded_topic = nullptr; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_allocator_t rcutils_allocator = rcutils_get_default_allocator(); + rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) { + throw_from_rcl_error(RCL_RET_BAD_ALLOC, "", rcutils_get_error_state(), rcutils_reset_error); + } else { + throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error); + } + } + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + if (ret != RCL_RET_OK) { + const rcutils_error_state_t * error_state = rcl_get_error_state(); + // finalize the string map before throwing + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to fini string_map (%d) during error handling: %s", + rcutils_ret, + rcutils_get_error_string().str); + rcutils_reset_error(); + } + throw_from_rcl_error(ret, "", error_state); + } + + ret = rcl_expand_topic_name( + name.c_str(), + node_name.c_str(), + namespace_.c_str(), + &substitutions_map, + allocator, + &expanded_topic); + + std::string result; + if (ret == RCL_RET_OK) { + result = expanded_topic; + allocator.deallocate(expanded_topic, allocator.state); + } + + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error); + } + + // expansion failed + if (ret != RCL_RET_OK) { + // if invalid topic or unknown substitution + if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + rcl_reset_error(); // explicitly discard error from rcl_expand_topic_name() + int validation_result; + size_t invalid_index; + rcl_ret_t ret = rcl_validate_topic_name(name.c_str(), &validation_result, &invalid_index); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret); + } + + if (validation_result != RCL_TOPIC_NAME_VALID) { + const char * validation_message = + rcl_topic_name_validation_result_string(validation_result); + if (is_service) { + using rclcpp::exceptions::InvalidServiceNameError; + throw InvalidServiceNameError(name.c_str(), validation_message, invalid_index); + } else { + using rclcpp::exceptions::InvalidTopicNameError; + throw InvalidTopicNameError(name.c_str(), validation_message, invalid_index); + } + } else { + throw std::runtime_error("topic name unexpectedly valid"); + } + + // if invalid node name + } else if (ret == RCL_RET_NODE_INVALID_NAME) { + rcl_reset_error(); // explicitly discard error from rcl_expand_topic_name() + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index); + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error( + RCL_RET_INVALID_ARGUMENT, "failed to validate node name", + rmw_get_error_state(), rmw_reset_error); + } + throw_from_rcl_error( + RCL_RET_ERROR, "failed to validate node name", + rmw_get_error_state(), rmw_reset_error); + } + + if (validation_result != RMW_NODE_NAME_VALID) { + throw rclcpp::exceptions::InvalidNodeNameError( + node_name.c_str(), + rmw_node_name_validation_result_string(validation_result), + invalid_index); + } else { + throw std::runtime_error("invalid rcl node name but valid rmw node name"); + } + + // if invalid namespace + } else if (ret == RCL_RET_NODE_INVALID_NAMESPACE) { + rcl_reset_error(); // explicitly discard error from rcl_expand_topic_name() + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index); + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error( + RCL_RET_INVALID_ARGUMENT, "failed to validate namespace", + rmw_get_error_state(), rmw_reset_error); + } + throw_from_rcl_error( + RCL_RET_ERROR, "failed to validate namespace", + rmw_get_error_state(), rmw_reset_error); + } + + if (validation_result != RMW_NAMESPACE_VALID) { + throw rclcpp::exceptions::InvalidNamespaceError( + namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); + } else { + throw std::runtime_error("invalid rcl namespace but valid rmw namespace"); + } + // something else happened + } else { + throw_from_rcl_error(ret); + } + } + + // expand succeeded, but full name validation may fail still + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_full_topic_name(result.c_str(), &validation_result, &invalid_index); + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error( + RCL_RET_INVALID_ARGUMENT, "failed to validate full topic name", + rmw_get_error_state(), rmw_reset_error); + } + throw_from_rcl_error( + RCL_RET_ERROR, "failed to validate full topic name", + rmw_get_error_state(), rmw_reset_error); + } + + if (validation_result != RMW_TOPIC_VALID) { + if (is_service) { + throw rclcpp::exceptions::InvalidServiceNameError( + result.c_str(), + rmw_full_topic_name_validation_result_string(validation_result), + invalid_index); + } else { + throw rclcpp::exceptions::InvalidTopicNameError( + result.c_str(), + rmw_full_topic_name_validation_result_string(validation_result), + invalid_index); + } + } + + return result; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/graph_listener.cpp new file mode 100644 index 0000000..b097564 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/graph_listener.cpp @@ -0,0 +1,404 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/graph_listener.hpp" + +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/types.h" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rmw/impl/cpp/demangle.hpp" + +#include "rcutils/logging_macros.h" + +using rclcpp::exceptions::throw_from_rcl_error; + +namespace rclcpp +{ +namespace graph_listener +{ + +GraphListener::GraphListener(std::shared_ptr parent_context) +: parent_context_(parent_context), + is_started_(false), + is_shutdown_(false), + interrupt_guard_condition_context_(nullptr), + shutdown_guard_condition_(nullptr) +{ + // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked + // automatically with the rcl guard condition + // hold on to this context to prevent it from going out of scope while this + // guard condition is using it. + interrupt_guard_condition_context_ = parent_context->get_rcl_context(); + rcl_ret_t ret = rcl_guard_condition_init( + &interrupt_guard_condition_, + interrupt_guard_condition_context_.get(), + rcl_guard_condition_get_default_options()); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to create interrupt guard condition"); + } + + shutdown_guard_condition_ = parent_context->get_interrupt_guard_condition(&wait_set_); +} + +GraphListener::~GraphListener() +{ + this->shutdown(std::nothrow); +} + +void +GraphListener::start_if_not_started() +{ + std::lock_guard shutdown_lock(shutdown_mutex_); + if (is_shutdown_.load()) { + throw GraphListenerShutdownError(); + } + if (!is_started_) { + // Initialize the wait set before starting. + rcl_ret_t ret = rcl_wait_set_init( + &wait_set_, + 0, // number_of_subscriptions + 2, // number_of_guard_conditions + 0, // number_of_timers + 0, // number_of_clients + 0, // number_of_services + 0, // number_of_events + this->parent_context_->get_rcl_context().get(), + rcl_get_default_allocator()); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to initialize wait set"); + } + // Register an on_shutdown hook to shtudown the graph listener. + // This is important to ensure that the wait set is finalized before + // destruction of static objects occurs. + std::weak_ptr weak_this = shared_from_this(); + rclcpp::on_shutdown( + [weak_this]() { + auto shared_this = weak_this.lock(); + if (shared_this) { + // should not throw from on_shutdown if it can be avoided + shared_this->shutdown(std::nothrow); + } + }); + // Start the listener thread. + listener_thread_ = std::thread(&GraphListener::run, this); + is_started_ = true; + } +} + +void +GraphListener::run() +{ + try { + run_loop(); + } catch (const std::exception & exc) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "caught %s exception in GraphListener thread: %s", + rmw::impl::cpp::demangle(exc).c_str(), + exc.what()); + std::rethrow_exception(std::current_exception()); + } catch (...) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "unknown error in GraphListener thread"); + std::rethrow_exception(std::current_exception()); + } +} + +void +GraphListener::run_loop() +{ + while (true) { + // If shutdown() was called, exit. + if (is_shutdown_.load()) { + return; + } + rcl_ret_t ret; + { + // This "barrier" lock ensures that other functions can acquire the + // node_graph_interfaces_mutex_ after waking up rcl_wait. + std::lock_guard nodes_barrier_lock(node_graph_interfaces_barrier_mutex_); + // This is ownership is passed to nodes_lock in the next line. + node_graph_interfaces_mutex_.lock(); + } + // This lock is released when the loop continues or exits. + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); + + // Resize the wait set if necessary. + const size_t node_graph_interfaces_size = node_graph_interfaces_.size(); + // Add 2 for the interrupt and shutdown guard conditions + if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_size + 2)) { + ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0, 0); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to resize wait set"); + } + } + // Clear the wait set. + ret = rcl_wait_set_clear(&wait_set_); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to clear wait set"); + } + // Put the interrupt guard condition in the wait set. + ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set"); + } + // Put the shutdown guard condition in the wait set. + size_t shutdown_guard_condition_index = 0u; + ret = rcl_wait_set_add_guard_condition( + &wait_set_, shutdown_guard_condition_, &shutdown_guard_condition_index); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set"); + } + // Put graph guard conditions for each node into the wait set. + std::vector graph_gc_indexes(node_graph_interfaces_size, 0u); + for (size_t i = 0u; i < node_graph_interfaces_size; ++i) { + auto node_ptr = node_graph_interfaces_[i]; + // Only wait on graph changes if some user of the node is watching. + if (node_ptr->count_graph_users() == 0) { + continue; + } + // Add the graph guard condition for the node to the wait set. + auto graph_gc = node_ptr->get_graph_guard_condition(); + if (!graph_gc) { + throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); + } + ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc, &graph_gc_indexes[i]); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to add graph guard condition to wait set"); + } + } + + // Wait for: graph changes, interrupt, or shutdown/SIGINT + ret = rcl_wait(&wait_set_, -1); // block for ever until a guard condition is triggered + if (RCL_RET_TIMEOUT == ret) { + throw std::runtime_error("rcl_wait unexpectedly timed out"); + } + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to wait on wait set"); + } + + // Check to see if the shutdown guard condition has been triggered. + bool shutdown_guard_condition_triggered = + (shutdown_guard_condition_ == wait_set_.guard_conditions[shutdown_guard_condition_index]); + // Notify nodes who's guard conditions are set (triggered). + for (size_t i = 0u; i < node_graph_interfaces_size; ++i) { + const auto node_ptr = node_graph_interfaces_[i]; + auto graph_gc = node_ptr->get_graph_guard_condition(); + if (!graph_gc) { + throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); + } + if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) { + node_ptr->notify_graph_change(); + } + if (shutdown_guard_condition_triggered) { + // If shutdown, then notify the node of this as well. + node_ptr->notify_shutdown(); + } + } + } // while (true) +} + +static void +interrupt_(rcl_guard_condition_t * interrupt_guard_condition) +{ + rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to trigger the interrupt guard condition"); + } +} + +static void +acquire_nodes_lock_( + std::mutex * node_graph_interfaces_barrier_mutex, + std::mutex * node_graph_interfaces_mutex, + rcl_guard_condition_t * interrupt_guard_condition) +{ + { + // Acquire this lock to prevent the run loop from re-locking the + // nodes_mutext_ after being woken up. + std::lock_guard nodes_barrier_lock(*node_graph_interfaces_barrier_mutex); + // Trigger the interrupt guard condition to wake up rcl_wait. + interrupt_(interrupt_guard_condition); + node_graph_interfaces_mutex->lock(); + } +} + +static bool +has_node_( + std::vector * node_graph_interfaces, + rclcpp::node_interfaces::NodeGraphInterface * node_graph) +{ + for (const auto node_ptr : (*node_graph_interfaces)) { + if (node_graph == node_ptr) { + return true; + } + } + return false; +} + +bool +GraphListener::has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) +{ + if (!node_graph) { + return false; + } + // Acquire the nodes mutex using the barrier to prevent the run loop from + // re-locking the nodes mutex after being interrupted. + acquire_nodes_lock_( + &node_graph_interfaces_barrier_mutex_, + &node_graph_interfaces_mutex_, + &interrupt_guard_condition_); + // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock. + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); + return has_node_(&node_graph_interfaces_, node_graph); +} + +void +GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) +{ + if (!node_graph) { + throw std::invalid_argument("node is nullptr"); + } + std::lock_guard shutdown_lock(shutdown_mutex_); + if (is_shutdown_.load()) { + throw GraphListenerShutdownError(); + } + + // Acquire the nodes mutex using the barrier to prevent the run loop from + // re-locking the nodes mutex after being interrupted. + acquire_nodes_lock_( + &node_graph_interfaces_barrier_mutex_, + &node_graph_interfaces_mutex_, + &interrupt_guard_condition_); + // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock. + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); + if (has_node_(&node_graph_interfaces_, node_graph)) { + throw NodeAlreadyAddedError(); + } + node_graph_interfaces_.push_back(node_graph); + // The run loop has already been interrupted by acquire_nodes_lock_() and + // will evaluate the new node when nodes_lock releases the node_graph_interfaces_mutex_. +} + +static void +remove_node_( + std::vector * node_graph_interfaces, + rclcpp::node_interfaces::NodeGraphInterface * node_graph) +{ + // Remove the node if it is found. + for (auto it = node_graph_interfaces->begin(); it != node_graph_interfaces->end(); ++it) { + if (node_graph == *it) { + // Found the node, remove it. + node_graph_interfaces->erase(it); + // Now trigger the interrupt guard condition to make sure + return; + } + } + // Not found in the loop. + throw NodeNotFoundError(); +} + +void +GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) +{ + if (!node_graph) { + throw std::invalid_argument("node is nullptr"); + } + std::lock_guard shutdown_lock(shutdown_mutex_); + if (is_shutdown()) { + // If shutdown, then the run loop has been joined, so we can remove them directly. + return remove_node_(&node_graph_interfaces_, node_graph); + } + // Otherwise, first interrupt and lock against the run loop to safely remove the node. + // Acquire the nodes mutex using the barrier to prevent the run loop from + // re-locking the nodes mutex after being interrupted. + acquire_nodes_lock_( + &node_graph_interfaces_barrier_mutex_, + &node_graph_interfaces_mutex_, + &interrupt_guard_condition_); + // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock. + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); + remove_node_(&node_graph_interfaces_, node_graph); +} + +void +GraphListener::__shutdown(bool should_throw) +{ + std::lock_guard shutdown_lock(shutdown_mutex_); + if (!is_shutdown_.exchange(true)) { + if (is_started_) { + interrupt_(&interrupt_guard_condition_); + listener_thread_.join(); + } + rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_); + interrupt_guard_condition_context_.reset(); // release context guard condition was using + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); + } + if (shutdown_guard_condition_) { + if (should_throw) { + parent_context_->release_interrupt_guard_condition(&wait_set_); + } else { + parent_context_->release_interrupt_guard_condition(&wait_set_, std::nothrow); + } + shutdown_guard_condition_ = nullptr; + } + if (is_started_) { + ret = rcl_wait_set_fini(&wait_set_); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to finalize wait set"); + } + } + } +} + +void +GraphListener::shutdown() +{ + this->__shutdown(true); +} + +void +GraphListener::shutdown(const std::nothrow_t &) noexcept +{ + try { + this->__shutdown(false); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught %s exception when shutting down GraphListener: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught unknown exception when shutting down GraphListener"); + } +} + +bool +GraphListener::is_shutdown() +{ + return is_shutdown_.load(); +} + +} // namespace graph_listener +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/init_options.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/init_options.cpp new file mode 100644 index 0000000..ba46edb --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/init_options.cpp @@ -0,0 +1,89 @@ +// Copyright 2018 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 "rclcpp/init_options.hpp" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" + +namespace rclcpp +{ + +InitOptions::InitOptions(rcl_allocator_t allocator) +: init_options_(new rcl_init_options_t) +{ + *init_options_ = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options"); + } +} + +InitOptions::InitOptions(const rcl_init_options_t & init_options) +: init_options_(new rcl_init_options_t) +{ + *init_options_ = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_copy(&init_options, init_options_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); + } +} + +InitOptions::InitOptions(const InitOptions & other) +: InitOptions(*other.get_rcl_init_options()) +{ + shutdown_on_sigint = other.shutdown_on_sigint; +} + +InitOptions & +InitOptions::operator=(const InitOptions & other) +{ + if (this != &other) { + this->finalize_init_options(); + rcl_ret_t ret = rcl_init_options_copy(other.get_rcl_init_options(), init_options_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); + } + this->shutdown_on_sigint = other.shutdown_on_sigint; + } + return *this; +} + +InitOptions::~InitOptions() +{ + this->finalize_init_options(); +} + +void +InitOptions::finalize_init_options() +{ + if (init_options_) { + rcl_ret_t ret = rcl_init_options_fini(init_options_.get()); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize rcl init options: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + *init_options_ = rcl_get_zero_initialized_init_options(); + } +} + +const rcl_init_options_t * +InitOptions::get_rcl_init_options() const +{ + return init_options_.get(); +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/intra_process_manager.cpp new file mode 100644 index 0000000..0b9c9d6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -0,0 +1,227 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/experimental/intra_process_manager.hpp" + +#include +#include +#include + +namespace rclcpp +{ +namespace experimental +{ + +static std::atomic _next_unique_id {1}; + +IntraProcessManager::IntraProcessManager() +{} + +IntraProcessManager::~IntraProcessManager() +{} + +uint64_t +IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) +{ + std::unique_lock lock(mutex_); + + auto id = IntraProcessManager::get_next_unique_id(); + + publishers_[id].publisher = publisher; + publishers_[id].topic_name = publisher->get_topic_name(); + publishers_[id].qos = publisher->get_actual_qos().get_rmw_qos_profile(); + + // Initialize the subscriptions storage for this publisher. + pub_to_subs_[id] = SplittedSubscriptions(); + + // create an entry for the publisher id and populate with already existing subscriptions + for (auto & pair : subscriptions_) { + if (can_communicate(publishers_[id], pair.second)) { + insert_sub_id_for_pub(pair.first, id, pair.second.use_take_shared_method); + } + } + + return id; +} + +uint64_t +IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription) +{ + std::unique_lock lock(mutex_); + + auto id = IntraProcessManager::get_next_unique_id(); + + subscriptions_[id].subscription = subscription; + subscriptions_[id].topic_name = subscription->get_topic_name(); + subscriptions_[id].qos = subscription->get_actual_qos(); + subscriptions_[id].use_take_shared_method = subscription->use_take_shared_method(); + + // adds the subscription id to all the matchable publishers + for (auto & pair : publishers_) { + if (can_communicate(pair.second, subscriptions_[id])) { + insert_sub_id_for_pub(id, pair.first, subscriptions_[id].use_take_shared_method); + } + } + + return id; +} + +void +IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) +{ + std::unique_lock lock(mutex_); + + subscriptions_.erase(intra_process_subscription_id); + + for (auto & pair : pub_to_subs_) { + pair.second.take_shared_subscriptions.erase( + std::remove( + pair.second.take_shared_subscriptions.begin(), + pair.second.take_shared_subscriptions.end(), + intra_process_subscription_id), + pair.second.take_shared_subscriptions.end()); + + pair.second.take_ownership_subscriptions.erase( + std::remove( + pair.second.take_ownership_subscriptions.begin(), + pair.second.take_ownership_subscriptions.end(), + intra_process_subscription_id), + pair.second.take_ownership_subscriptions.end()); + } +} + +void +IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) +{ + std::unique_lock lock(mutex_); + + publishers_.erase(intra_process_publisher_id); + pub_to_subs_.erase(intra_process_publisher_id); +} + +bool +IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const +{ + std::shared_lock lock(mutex_); + + for (auto & publisher_pair : publishers_) { + auto publisher = publisher_pair.second.publisher.lock(); + if (!publisher) { + continue; + } + if (*publisher.get() == id) { + return true; + } + } + return false; +} + +size_t +IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const +{ + std::shared_lock lock(mutex_); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling get_subscription_count for invalid or no longer existing publisher id"); + return 0; + } + + auto count = + publisher_it->second.take_shared_subscriptions.size() + + publisher_it->second.take_ownership_subscriptions.size(); + + return count; +} + +SubscriptionIntraProcessBase::SharedPtr +IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subscription_id) +{ + std::shared_lock lock(mutex_); + + auto subscription_it = subscriptions_.find(intra_process_subscription_id); + if (subscription_it == subscriptions_.end()) { + return nullptr; + } else { + return subscription_it->second.subscription; + } +} + +uint64_t +IntraProcessManager::get_next_unique_id() +{ + auto next_id = _next_unique_id.fetch_add(1, std::memory_order_relaxed); + // Check for rollover (we started at 1). + if (0 == next_id) { + // This puts a technical limit on the number of times you can add a publisher or subscriber. + // But even if you could add (and remove) them at 1 kHz (very optimistic rate) + // it would still be a very long time before you could exhaust the pool of id's: + // 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years + // So around 585 million years. Even at 1 GHz, it would take 585 years. + // I think it's safe to avoid trying to handle overflow. + // If we roll over then it's most likely a bug. + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::overflow_error( + "exhausted the unique id's for publishers and subscribers in this process " + "(congratulations your computer is either extremely fast or extremely old)"); + // *INDENT-ON* + } + return next_id; +} + +void +IntraProcessManager::insert_sub_id_for_pub( + uint64_t sub_id, + uint64_t pub_id, + bool use_take_shared_method) +{ + if (use_take_shared_method) { + pub_to_subs_[pub_id].take_shared_subscriptions.push_back(sub_id); + } else { + pub_to_subs_[pub_id].take_ownership_subscriptions.push_back(sub_id); + } +} + +bool +IntraProcessManager::can_communicate( + PublisherInfo pub_info, + SubscriptionInfo sub_info) const +{ + // publisher and subscription must be on the same topic + if (strcmp(pub_info.topic_name, sub_info.topic_name) != 0) { + return false; + } + + // TODO(alsora): the following checks for qos compatibility should be provided by the RMW + // a reliable subscription can't be connected with a best effort publisher + if ( + sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE && + pub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + { + return false; + } + + // a publisher and a subscription with different durability can't communicate + if (sub_info.qos.durability != pub_info.qos.durability) { + return false; + } + + return true; +} + +} // namespace experimental +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/logger.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/logger.cpp new file mode 100644 index 0000000..b3d25c0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/logger.cpp @@ -0,0 +1,47 @@ +// Copyright 2017 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 "rclcpp/logger.hpp" + +#include "rclcpp/logging.hpp" + +namespace rclcpp +{ + +Logger +get_logger(const std::string & name) +{ +#if RCLCPP_LOGGING_ENABLED + return rclcpp::Logger(name); +#else + (void)name; + return rclcpp::Logger(); +#endif +} + +Logger +get_node_logger(const rcl_node_t * node) +{ + const char * logger_name = rcl_node_get_logger_name(node); + if (nullptr == logger_name) { + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_ERROR(logger, "failed to get logger name from node at address %p", node); + return logger; + } + return rclcpp::get_logger(logger_name); +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategies.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategies.cpp new file mode 100644 index 0000000..4c82dbb --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategies.cpp @@ -0,0 +1,27 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/memory_strategies.hpp" + +#include + +#include "rclcpp/strategies/allocator_memory_strategy.hpp" + +using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; + +rclcpp::memory_strategy::MemoryStrategy::SharedPtr +rclcpp::memory_strategies::create_default_strategy() +{ + return std::make_shared>(); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategy.cpp new file mode 100644 index 0000000..5a2a5da --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/memory_strategy.cpp @@ -0,0 +1,284 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/memory_strategy.hpp" +#include + +using rclcpp::memory_strategy::MemoryStrategy; + +rclcpp::SubscriptionBase::SharedPtr +MemoryStrategy::get_subscription_by_handle( + std::shared_ptr subscriber_handle, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto match_subscription = group->find_subscription_ptrs_if( + [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool { + return subscription->get_subscription_handle() == subscriber_handle; + }); + if (match_subscription) { + return match_subscription; + } + } + } + return nullptr; +} + +rclcpp::ServiceBase::SharedPtr +MemoryStrategy::get_service_by_handle( + std::shared_ptr service_handle, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto service_ref = group->find_service_ptrs_if( + [&service_handle](const rclcpp::ServiceBase::SharedPtr & service) -> bool { + return service->get_service_handle() == service_handle; + }); + if (service_ref) { + return service_ref; + } + } + } + return nullptr; +} + +rclcpp::ClientBase::SharedPtr +MemoryStrategy::get_client_by_handle( + std::shared_ptr client_handle, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto client_ref = group->find_client_ptrs_if( + [&client_handle](const rclcpp::ClientBase::SharedPtr & client) -> bool { + return client->get_client_handle() == client_handle; + }); + if (client_ref) { + return client_ref; + } + } + } + return nullptr; +} + +rclcpp::TimerBase::SharedPtr +MemoryStrategy::get_timer_by_handle( + std::shared_ptr timer_handle, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto timer_ref = group->find_timer_ptrs_if( + [&timer_handle](const rclcpp::TimerBase::SharedPtr & timer) -> bool { + return timer->get_timer_handle() == timer_handle; + }); + if (timer_ref) { + return timer_ref; + } + } + } + return nullptr; +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +MemoryStrategy::get_node_by_group( + rclcpp::callback_group::CallbackGroup::SharedPtr group, + const WeakNodeList & weak_nodes) +{ + if (!group) { + return nullptr; + } + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto callback_group = weak_group.lock(); + if (callback_group == group) { + return node; + } + } + } + return nullptr; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto match_sub = group->find_subscription_ptrs_if( + [&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool { + return sub == subscription; + }); + if (match_sub) { + return group; + } + } + } + return nullptr; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_service( + rclcpp::ServiceBase::SharedPtr service, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto service_ref = group->find_service_ptrs_if( + [&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool { + return serv == service; + }); + if (service_ref) { + return group; + } + } + } + return nullptr; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_client( + rclcpp::ClientBase::SharedPtr client, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto client_ref = group->find_client_ptrs_if( + [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool { + return cli == client; + }); + if (client_ref) { + return group; + } + } + } + return nullptr; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_timer( + rclcpp::TimerBase::SharedPtr timer, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto timer_ref = group->find_timer_ptrs_if( + [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool { + return time == timer; + }); + if (timer_ref) { + return group; + } + } + } + return nullptr; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_waitable( + rclcpp::Waitable::SharedPtr waitable, + const WeakNodeList & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + auto waitable_ref = group->find_waitable_ptrs_if( + [&waitable](const rclcpp::Waitable::SharedPtr & group_waitable) -> bool { + return group_waitable == waitable; + }); + if (waitable_ref) { + return group; + } + } + } + return nullptr; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node.cpp new file mode 100644 index 0000000..783a7e1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node.cpp @@ -0,0 +1,489 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/graph_listener.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_clock.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/node_interfaces/node_logging.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" +#include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_time_source.hpp" +#include "rclcpp/node_interfaces/node_timers.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" + +#include "rmw/validate_namespace.h" + +using rclcpp::Node; +using rclcpp::NodeOptions; +using rclcpp::exceptions::throw_from_rcl_error; + +RCLCPP_LOCAL +std::string +extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension) +{ + // Assumption is that the existing_sub_namespace does not need checking + // because it would be checked already when it was set with this function. + + // check if the new sub-namespace extension is absolute + if (extension.front() == '/') { + throw rclcpp::exceptions::NameValidationError( + "sub_namespace", + extension.c_str(), + "a sub-namespace should not have a leading /", + 0); + } + + std::string new_sub_namespace; + if (existing_sub_namespace.empty()) { + new_sub_namespace = extension; + } else { + new_sub_namespace = existing_sub_namespace + "/" + extension; + } + + // remove any trailing `/` so that new extensions do no result in `//` + if (new_sub_namespace.back() == '/') { + new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1); + } + + return new_sub_namespace; +} + +RCLCPP_LOCAL +std::string +create_effective_namespace(const std::string & node_namespace, const std::string & sub_namespace) +{ + // Assumption is that both the node_namespace and sub_namespace are conforming + // and do not need trimming of `/` and other things, as they were validated + // in other functions already. + + if (node_namespace.back() == '/') { + // this is the special case where node_namespace is just `/` + return node_namespace + sub_namespace; + } else { + return node_namespace + "/" + sub_namespace; + } +} + +Node::Node( + const std::string & node_name, + const NodeOptions & options) +: Node(node_name, "", options) +{ +} + +Node::Node( + const std::string & node_name, + const std::string & namespace_, + const NodeOptions & options) +: node_base_(new rclcpp::node_interfaces::NodeBase( + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms())), + node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), + node_clock_(new rclcpp::node_interfaces::NodeClock( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_ + )), + node_parameters_(new rclcpp::node_interfaces::NodeParameters( + node_base_, + node_logging_, + node_topics_, + node_services_, + node_clock_, + options.parameter_overrides(), + options.start_parameter_services(), + options.start_parameter_event_publisher(), + options.parameter_event_qos(), + options.parameter_event_publisher_options(), + options.allow_undeclared_parameters(), + options.automatically_declare_parameters_from_overrides() + )), + node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_ + )), + node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), + node_options_(options), + sub_namespace_(""), + effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_)) +{ +} + +Node::Node( + const Node & other, + const std::string & sub_namespace) +: node_base_(other.node_base_), + node_graph_(other.node_graph_), + node_logging_(other.node_logging_), + node_timers_(other.node_timers_), + node_topics_(other.node_topics_), + node_services_(other.node_services_), + node_clock_(other.node_clock_), + node_parameters_(other.node_parameters_), + node_options_(other.node_options_), + sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)), + effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)) +{ + // Validate new effective namespace. + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_namespace(effective_namespace_.c_str(), &validation_result, &invalid_index); + + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate subnode namespace"); + } + throw_from_rcl_error(RCL_RET_ERROR, "failed to validate subnode namespace"); + } + + if (validation_result != RMW_NAMESPACE_VALID) { + throw rclcpp::exceptions::InvalidNamespaceError( + effective_namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); + } +} + +Node::~Node() +{} + +const char * +Node::get_name() const +{ + return node_base_->get_name(); +} + +const char * +Node::get_namespace() const +{ + return node_base_->get_namespace(); +} + +const char * +Node::get_fully_qualified_name() const +{ + return node_base_->get_fully_qualified_name(); +} + +rclcpp::Logger +Node::get_logger() const +{ + return node_logging_->get_logger(); +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +Node::create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type) +{ + return node_base_->create_callback_group(group_type); +} + +bool +Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + return node_base_->callback_group_in_node(group); +} + +const rclcpp::ParameterValue & +Node::declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + return this->node_parameters_->declare_parameter( + name, + default_value, + parameter_descriptor, + ignore_override); +} + +void +Node::undeclare_parameter(const std::string & name) +{ + this->node_parameters_->undeclare_parameter(name); +} + +bool +Node::has_parameter(const std::string & name) const +{ + return this->node_parameters_->has_parameter(name); +} + +rcl_interfaces::msg::SetParametersResult +Node::set_parameter(const rclcpp::Parameter & parameter) +{ + return this->set_parameters_atomically({parameter}); +} + +std::vector +Node::set_parameters(const std::vector & parameters) +{ + return node_parameters_->set_parameters(parameters); +} + +rcl_interfaces::msg::SetParametersResult +Node::set_parameters_atomically(const std::vector & parameters) +{ + return node_parameters_->set_parameters_atomically(parameters); +} + +rclcpp::Parameter +Node::get_parameter(const std::string & name) const +{ + return node_parameters_->get_parameter(name); +} + +bool +Node::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const +{ + return node_parameters_->get_parameter(name, parameter); +} + +std::vector +Node::get_parameters( + const std::vector & names) const +{ + return node_parameters_->get_parameters(names); +} + +rcl_interfaces::msg::ParameterDescriptor +Node::describe_parameter(const std::string & name) const +{ + auto result = node_parameters_->describe_parameters({name}); + if (0 == result.size()) { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } + if (result.size() > 1) { + throw std::runtime_error("number of described parameters unexpectedly more than one"); + } + return result.front(); +} + +std::vector +Node::describe_parameters(const std::vector & names) const +{ + return node_parameters_->describe_parameters(names); +} + +std::vector +Node::get_parameter_types(const std::vector & names) const +{ + return node_parameters_->get_parameter_types(names); +} + +rcl_interfaces::msg::ListParametersResult +Node::list_parameters(const std::vector & prefixes, uint64_t depth) const +{ + return node_parameters_->list_parameters(prefixes, depth); +} + +rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr +Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +{ + return node_parameters_->add_on_set_parameters_callback(callback); +} + +void +Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) +{ + return node_parameters_->remove_on_set_parameters_callback(callback); +} + +rclcpp::Node::OnParametersSetCallbackType +Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) +{ + return node_parameters_->set_on_parameters_set_callback(callback); +} + +std::vector +Node::get_node_names() const +{ + return node_graph_->get_node_names(); +} + +std::map> +Node::get_topic_names_and_types() const +{ + return node_graph_->get_topic_names_and_types(); +} + +std::map> +Node::get_service_names_and_types() const +{ + return node_graph_->get_service_names_and_types(); +} + +size_t +Node::count_publishers(const std::string & topic_name) const +{ + return node_graph_->count_publishers(topic_name); +} + +size_t +Node::count_subscribers(const std::string & topic_name) const +{ + return node_graph_->count_subscribers(topic_name); +} + +const std::vector & +Node::get_callback_groups() const +{ + return node_base_->get_callback_groups(); +} + +rclcpp::Event::SharedPtr +Node::get_graph_event() +{ + return node_graph_->get_graph_event(); +} + +void +Node::wait_for_graph_change( + rclcpp::Event::SharedPtr event, + std::chrono::nanoseconds timeout) +{ + node_graph_->wait_for_graph_change(event, timeout); +} + +rclcpp::Clock::SharedPtr +Node::get_clock() +{ + return node_clock_->get_clock(); +} + +rclcpp::Time +Node::now() +{ + return node_clock_->get_clock()->now(); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +Node::get_node_base_interface() +{ + return node_base_; +} + +rclcpp::node_interfaces::NodeClockInterface::SharedPtr +Node::get_node_clock_interface() +{ + return node_clock_; +} + +rclcpp::node_interfaces::NodeGraphInterface::SharedPtr +Node::get_node_graph_interface() +{ + return node_graph_; +} + +rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr +Node::get_node_logging_interface() +{ + return node_logging_; +} + +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +Node::get_node_time_source_interface() +{ + return node_time_source_; +} + +rclcpp::node_interfaces::NodeTimersInterface::SharedPtr +Node::get_node_timers_interface() +{ + return node_timers_; +} + +rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr +Node::get_node_topics_interface() +{ + return node_topics_; +} + +rclcpp::node_interfaces::NodeServicesInterface::SharedPtr +Node::get_node_services_interface() +{ + return node_services_; +} + +rclcpp::node_interfaces::NodeParametersInterface::SharedPtr +Node::get_node_parameters_interface() +{ + return node_parameters_; +} + +rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr +Node::get_node_waitables_interface() +{ + return node_waitables_; +} + +const std::string & +Node::get_sub_namespace() const +{ + return this->sub_namespace_; +} + +const std::string & +Node::get_effective_namespace() const +{ + return this->effective_namespace_; +} + +Node::SharedPtr +Node::create_sub_node(const std::string & sub_namespace) +{ + // Cannot use make_shared() here as it requires the constructor to be + // public, and this constructor is intentionally protected instead. + return std::shared_ptr(new Node(*this, sub_namespace)); +} + +const NodeOptions & +Node::get_node_options() const +{ + return this->node_options_; +} + +bool +Node::assert_liveliness() const +{ + return node_base_->assert_liveliness(); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_base.cpp new file mode 100644 index 0000000..9241adf --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -0,0 +1,270 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/node_interfaces/node_base.hpp" + +#include "rcl/arguments.h" +#include "rclcpp/exceptions.hpp" +#include "rcutils/logging_macros.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +using rclcpp::exceptions::throw_from_rcl_error; + +using rclcpp::node_interfaces::NodeBase; + +NodeBase::NodeBase( + const std::string & node_name, + const std::string & namespace_, + rclcpp::Context::SharedPtr context, + const rcl_node_options_t & rcl_node_options, + bool use_intra_process_default) +: context_(context), + use_intra_process_default_(use_intra_process_default), + node_handle_(nullptr), + default_callback_group_(nullptr), + associated_with_executor_(false), + notify_guard_condition_is_valid_(false) +{ + // Setup the guard condition that is notified when changes occur in the graph. + rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); + rcl_ret_t ret = rcl_guard_condition_init( + ¬ify_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to create interrupt guard condition"); + } + + // Setup a safe exit lamda to clean up the guard condition in case of an error here. + auto finalize_notify_guard_condition = [this]() { + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string().str); + } + }; + + // Create the rcl node and store it in a shared_ptr with a custom destructor. + std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); + + ret = rcl_node_init( + rcl_node.get(), + node_name.c_str(), namespace_.c_str(), + context_->get_rcl_context().get(), &rcl_node_options); + if (ret != RCL_RET_OK) { + // Finalize the interrupt guard condition. + finalize_notify_guard_condition(); + + if (ret == RCL_RET_NODE_INVALID_NAME) { + rcl_reset_error(); // discard rcl_node_init error + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index); + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate node name"); + } + throw_from_rcl_error(RCL_RET_ERROR, "failed to validate node name"); + } + + if (validation_result != RMW_NODE_NAME_VALID) { + throw rclcpp::exceptions::InvalidNodeNameError( + node_name.c_str(), + rmw_node_name_validation_result_string(validation_result), + invalid_index); + } else { + throw std::runtime_error("valid rmw node name but invalid rcl node name"); + } + } + + if (ret == RCL_RET_NODE_INVALID_NAMESPACE) { + rcl_reset_error(); // discard rcl_node_init error + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index); + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate namespace"); + } + throw_from_rcl_error(RCL_RET_ERROR, "failed to validate namespace"); + } + + if (validation_result != RMW_NAMESPACE_VALID) { + throw rclcpp::exceptions::InvalidNamespaceError( + namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); + } else { + throw std::runtime_error("valid rmw node namespace but invalid rcl node namespace"); + } + } + throw_from_rcl_error(ret, "failed to initialize rcl node"); + } + + node_handle_.reset( + rcl_node.release(), + [](rcl_node_t * node) -> void { + if (rcl_node_fini(node) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); + } + delete node; + }); + + // Create the default callback group. + using rclcpp::callback_group::CallbackGroupType; + default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive); + + // Indicate the notify_guard_condition is now valid. + notify_guard_condition_is_valid_ = true; +} + +NodeBase::~NodeBase() +{ + // Finalize the interrupt guard condition after removing self from graph listener. + { + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + notify_guard_condition_is_valid_ = false; + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to destroy guard condition: %s", rcl_get_error_string().str); + } + } +} + +const char * +NodeBase::get_name() const +{ + return rcl_node_get_name(node_handle_.get()); +} + +const char * +NodeBase::get_namespace() const +{ + return rcl_node_get_namespace(node_handle_.get()); +} + +const char * +NodeBase::get_fully_qualified_name() const +{ + return rcl_node_get_fully_qualified_name(node_handle_.get()); +} + +rclcpp::Context::SharedPtr +NodeBase::get_context() +{ + return context_; +} + +rcl_node_t * +NodeBase::get_rcl_node_handle() +{ + return node_handle_.get(); +} + +const rcl_node_t * +NodeBase::get_rcl_node_handle() const +{ + return node_handle_.get(); +} + +std::shared_ptr +NodeBase::get_shared_rcl_node_handle() +{ + return node_handle_; +} + +std::shared_ptr +NodeBase::get_shared_rcl_node_handle() const +{ + return node_handle_; +} + +bool +NodeBase::assert_liveliness() const +{ + return RCL_RET_OK == rcl_node_assert_liveliness(get_rcl_node_handle()); +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) +{ + using rclcpp::callback_group::CallbackGroup; + using rclcpp::callback_group::CallbackGroupType; + auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); + callback_groups_.push_back(group); + return group; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +NodeBase::get_default_callback_group() +{ + return default_callback_group_; +} + +bool +NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + bool group_belongs_to_this_node = false; + for (auto & weak_group : this->callback_groups_) { + auto cur_group = weak_group.lock(); + if (cur_group && (cur_group == group)) { + group_belongs_to_this_node = true; + } + } + return group_belongs_to_this_node; +} + +const std::vector & +NodeBase::get_callback_groups() const +{ + return callback_groups_; +} + +std::atomic_bool & +NodeBase::get_associated_with_executor_atomic() +{ + return associated_with_executor_; +} + +rcl_guard_condition_t * +NodeBase::get_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + return nullptr; + } + return ¬ify_guard_condition_; +} + +std::unique_lock +NodeBase::acquire_notify_guard_condition_lock() const +{ + return std::unique_lock(notify_guard_condition_mutex_); +} + +bool +NodeBase::get_use_intra_process_default() const +{ + return use_intra_process_default_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp new file mode 100644 index 0000000..8de8c3c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -0,0 +1,43 @@ +// Copyright 2017 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 "rclcpp/node_interfaces/node_clock.hpp" + +#include +#include + +using rclcpp::node_interfaces::NodeClock; + +NodeClock::NodeClock( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging) +: node_base_(node_base), + node_topics_(node_topics), + node_graph_(node_graph), + node_services_(node_services), + node_logging_(node_logging), + ros_clock_(std::make_shared(RCL_ROS_TIME)) +{} + +NodeClock::~NodeClock() +{} + +std::shared_ptr +NodeClock::get_clock() +{ + return ros_clock_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp new file mode 100644 index 0000000..be56c71 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -0,0 +1,373 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/node_interfaces/node_graph.hpp" + +#include +#include +#include +#include +#include + +#include "rcl/graph.h" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/graph_listener.hpp" + +using rclcpp::node_interfaces::NodeGraph; +using rclcpp::exceptions::throw_from_rcl_error; +using rclcpp::graph_listener::GraphListener; + +NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base), + graph_listener_( + node_base->get_context()->get_sub_context(node_base->get_context()) + ), + should_add_to_graph_listener_(true), + graph_users_count_(0) +{} + +NodeGraph::~NodeGraph() +{ + // Remove self from graph listener. + // Exchange with false to prevent others from trying to add this node to the + // graph listener after checking that it was not here. + if (!should_add_to_graph_listener_.exchange(false)) { + // If it was already false, then it needs to now be removed. + graph_listener_->remove_node(this); + } +} + +std::map> +NodeGraph::get_topic_names_and_types(bool no_demangle) const +{ + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto ret = rcl_get_topic_names_and_types( + node_base_->get_rcl_node_handle(), + &allocator, + no_demangle, + &topic_names_and_types); + if (ret != RCL_RET_OK) { + auto error_msg = std::string("failed to get topic names and types: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { + error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") + + rcl_get_error_string().str; + } + throw std::runtime_error(error_msg + rcl_get_error_string().str); + } + + std::map> topics_and_types; + for (size_t i = 0; i < topic_names_and_types.names.size; ++i) { + std::string topic_name = topic_names_and_types.names.data[i]; + for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) { + topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]); + } + } + + ret = rcl_names_and_types_fini(&topic_names_and_types); + if (ret != RCL_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not destroy topic names and types: ") + rcl_get_error_string().str); + // *INDENT-ON* + } + + return topics_and_types; +} + +std::map> +NodeGraph::get_service_names_and_types() const +{ + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto ret = rcl_get_service_names_and_types( + node_base_->get_rcl_node_handle(), + &allocator, + &service_names_and_types); + if (ret != RCL_RET_OK) { + auto error_msg = std::string("failed to get service names and types: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { + error_msg += + std::string(", failed also to cleanup service names and types, leaking memory: ") + + rcl_get_error_string().str; + } + throw std::runtime_error(error_msg + rcl_get_error_string().str); + } + + std::map> services_and_types; + for (size_t i = 0; i < service_names_and_types.names.size; ++i) { + std::string service_name = service_names_and_types.names.data[i]; + for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) { + services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]); + } + } + + ret = rcl_names_and_types_fini(&service_names_and_types); + if (ret != RCL_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not destroy service names and types: ") + rcl_get_error_string().str); + // *INDENT-ON* + } + + return services_and_types; +} + +std::vector +NodeGraph::get_node_names() const +{ + std::vector nodes; + auto names_and_namespaces = get_node_names_and_namespaces(); + + std::transform( + names_and_namespaces.begin(), + names_and_namespaces.end(), + std::back_inserter(nodes), + [](std::pair nns) { + std::string return_string; + if (nns.second.back() == '/') { + return_string = nns.second + nns.first; + } else { + return_string = nns.second + '/' + nns.first; + } + // Quick check to make sure that we start with a slash + // Since fully-qualified strings need to + if (return_string.front() != '/') { + return_string = "/" + return_string; + } + return return_string; + } + ); + return nodes; +} + +std::vector> +NodeGraph::get_node_names_and_namespaces() const +{ + rcutils_string_array_t node_names_c = + rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces_c = + rcutils_get_zero_initialized_string_array(); + + auto allocator = rcl_get_default_allocator(); + auto ret = rcl_get_node_names( + node_base_->get_rcl_node_handle(), + allocator, + &node_names_c, + &node_namespaces_c); + if (ret != RCL_RET_OK) { + auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str; + rcl_reset_error(); + if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node names, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + // TODO(karsten1987): Append rcutils_error_message once it's in master + throw std::runtime_error(error_msg); + } + + + std::vector> node_names; + node_names.reserve(node_names_c.size); + for (size_t i = 0; i < node_names_c.size; ++i) { + if (node_names_c.data[i] && node_namespaces_c.data[i]) { + node_names.emplace_back(node_names_c.data[i], node_namespaces_c.data[i]); + } + } + + std::string error; + rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c); + if (ret_names != RCUTILS_RET_OK) { + // *INDENT-OFF* + // TODO(karsten1987): Append rcutils_error_message once it's in master + error = "could not destroy node names"; + // *INDENT-ON* + } + rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c); + if (ret_ns != RCUTILS_RET_OK) { + // *INDENT-OFF* + // TODO(karsten1987): Append rcutils_error_message once it's in master + error += ", could not destroy node namespaces"; + // *INDENT-ON* + } + + if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK) { + throw std::runtime_error(error); + } + + return node_names; +} + +size_t +NodeGraph::count_publishers(const std::string & topic_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + false); // false = not a service + + size_t count; + auto ret = rcl_count_publishers(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count publishers: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + +size_t +NodeGraph::count_subscribers(const std::string & topic_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + false); // false = not a service + + size_t count; + auto ret = rcl_count_subscribers(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count subscribers: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + +const rcl_guard_condition_t * +NodeGraph::get_graph_guard_condition() const +{ + return rcl_node_get_graph_guard_condition(node_base_->get_rcl_node_handle()); +} + +void +NodeGraph::notify_graph_change() +{ + { + std::lock_guard graph_changed_lock(graph_mutex_); + bool bad_ptr_encountered = false; + for (auto & event_wptr : graph_events_) { + auto event_ptr = event_wptr.lock(); + if (event_ptr) { + event_ptr->set(); + } else { + bad_ptr_encountered = true; + } + } + if (bad_ptr_encountered) { + // remove invalid pointers with the erase-remove idiom + graph_events_.erase( + std::remove_if( + graph_events_.begin(), + graph_events_.end(), + [](const rclcpp::Event::WeakPtr & wptr) { + return wptr.expired(); + }), + graph_events_.end()); + // update graph_users_count_ + graph_users_count_.store(graph_events_.size()); + } + } + graph_cv_.notify_all(); + { + auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to trigger notify guard condition"); + } + } +} + +void +NodeGraph::notify_shutdown() +{ + // notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown(). + graph_cv_.notify_all(); +} + +rclcpp::Event::SharedPtr +NodeGraph::get_graph_event() +{ + auto event = rclcpp::Event::make_shared(); + { + std::lock_guard graph_changed_lock(graph_mutex_); + graph_events_.push_back(event); + graph_users_count_++; + } + // on first call, add node to graph_listener_ + if (should_add_to_graph_listener_.exchange(false)) { + graph_listener_->add_node(this); + graph_listener_->start_if_not_started(); + } + return event; +} + +void +NodeGraph::wait_for_graph_change( + rclcpp::Event::SharedPtr event, + std::chrono::nanoseconds timeout) +{ + using rclcpp::exceptions::InvalidEventError; + using rclcpp::exceptions::EventNotRegisteredError; + if (!event) { + throw InvalidEventError(); + } + { + std::lock_guard graph_changed_lock(graph_mutex_); + bool event_in_graph_events = false; + for (const auto & event_wptr : graph_events_) { + if (event == event_wptr.lock()) { + event_in_graph_events = true; + break; + } + } + if (!event_in_graph_events) { + throw EventNotRegisteredError(); + } + } + auto pred = [&event, context = node_base_->get_context()]() { + return event->check() || !rclcpp::ok(context); + }; + std::unique_lock graph_lock(graph_mutex_); + if (!pred()) { + graph_cv_.wait_for(graph_lock, timeout, pred); + } +} + +size_t +NodeGraph::count_graph_users() +{ + return graph_users_count_.load(); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp new file mode 100644 index 0000000..269f14d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp @@ -0,0 +1,39 @@ +// Copyright 2017 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 "rclcpp/node_interfaces/node_logging.hpp" + +using rclcpp::node_interfaces::NodeLogging; + +NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{ + logger_ = rclcpp::get_logger(this->get_logger_name()); +} + +NodeLogging::~NodeLogging() +{ +} + +rclcpp::Logger +NodeLogging::get_logger() const +{ + return logger_; +} + +const char * +NodeLogging::get_logger_name() const +{ + return rcl_node_get_logger_name(node_base_->get_rcl_node_handle()); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp new file mode 100644 index 0000000..09233a9 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -0,0 +1,890 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/node_interfaces/node_parameters.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/parameter_map.hpp" +#include "rclcpp/scope_exit.hpp" +#include "rcutils/logging_macros.h" +#include "rmw/qos_profiles.h" + +using rclcpp::node_interfaces::NodeParameters; + +NodeParameters::NodeParameters( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + const std::vector & parameter_overrides, + bool start_parameter_services, + bool start_parameter_event_publisher, + const rclcpp::QoS & parameter_event_qos, + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, + bool allow_undeclared_parameters, + bool automatically_declare_parameters_from_overrides) +: allow_undeclared_(allow_undeclared_parameters), + events_publisher_(nullptr), + node_logging_(node_logging), + node_clock_(node_clock) +{ + using MessageT = rcl_interfaces::msg::ParameterEvent; + using PublisherT = rclcpp::Publisher; + using AllocatorT = std::allocator; + // TODO(wjwwood): expose this allocator through the Parameter interface. + rclcpp::PublisherOptionsWithAllocator publisher_options( + parameter_event_publisher_options); + publisher_options.allocator = std::make_shared(); + + if (start_parameter_services) { + parameter_service_ = std::make_shared(node_base, node_services, this); + } + + if (start_parameter_event_publisher) { + events_publisher_ = rclcpp::create_publisher( + node_topics, + "parameter_events", + parameter_event_qos, + publisher_options); + } + + // Get the node options + const rcl_node_t * node = node_base->get_rcl_node_handle(); + if (nullptr == node) { + throw std::runtime_error("Need valid node handle in NodeParameters"); + } + const rcl_node_options_t * options = rcl_node_get_options(node); + if (nullptr == options) { + throw std::runtime_error("Need valid node options in NodeParameters"); + } + + std::vector argument_sources; + // global before local so that local overwrites global + if (options->use_global_arguments) { + auto context_ptr = node_base->get_context()->get_rcl_context(); + argument_sources.push_back(&(context_ptr->global_arguments)); + } + argument_sources.push_back(&options->arguments); + + // Get fully qualified node name post-remapping to use to find node's params in yaml files + combined_name_ = node_base->get_fully_qualified_name(); + + for (const rcl_arguments_t * source : argument_sources) { + rcl_params_t * params = NULL; + rcl_ret_t ret = rcl_arguments_get_param_overrides(source, ¶ms); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + if (params) { + auto cleanup_params = make_scope_exit( + [params]() { + rcl_yaml_node_struct_fini(params); + }); + rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); + for (auto iter = initial_map.begin(); initial_map.end() != iter; iter++) { + // TODO(cottsay) implement further wildcard matching + if (iter->first == "/**" || iter->first == combined_name_) { + // Combine parameter yaml files, overwriting values in older ones + for (auto & param : iter->second) { + parameter_overrides_[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); + } + } + } + } + } + + // parameter overrides passed to constructor will overwrite overrides from yaml file sources + for (auto & param : parameter_overrides) { + parameter_overrides_[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); + } + + // If asked, initialize any parameters that ended up in the initial parameter values, + // but did not get declared explcitily by this point. + if (automatically_declare_parameters_from_overrides) { + for (const auto & pair : this->get_parameter_overrides()) { + if (!this->has_parameter(pair.first)) { + this->declare_parameter( + pair.first, + pair.second, + rcl_interfaces::msg::ParameterDescriptor(), + true); + } + } + } +} + +NodeParameters::~NodeParameters() +{} + +RCLCPP_LOCAL +bool +__lockless_has_parameter( + const std::map & parameters, + const std::string & name) +{ + return parameters.find(name) != parameters.end(); +} + +// see https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon +RCLCPP_LOCAL +bool +__are_doubles_equal(double x, double y, size_t ulp = 100) +{ + return std::abs(x - y) <= std::numeric_limits::epsilon() * std::abs(x + y) * ulp; +} + +RCLCPP_LOCAL +inline +void +format_reason(std::string & reason, const std::string & name, const char * range_type) +{ + std::ostringstream ss; + ss << "Parameter {" << name << "} doesn't comply with " << range_type << " range."; + reason = ss.str(); +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__check_parameter_value_in_range( + const rcl_interfaces::msg::ParameterDescriptor & descriptor, + const rclcpp::ParameterValue & value) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (!descriptor.integer_range.empty() && value.get_type() == rclcpp::PARAMETER_INTEGER) { + int64_t v = value.get(); + auto integer_range = descriptor.integer_range.at(0); + if (v == integer_range.from_value || v == integer_range.to_value) { + return result; + } + if ((v < integer_range.from_value) || (v > integer_range.to_value)) { + result.successful = false; + format_reason(result.reason, descriptor.name, "integer"); + return result; + } + if (integer_range.step == 0) { + return result; + } + if (((v - integer_range.from_value) % integer_range.step) == 0) { + return result; + } + result.successful = false; + format_reason(result.reason, descriptor.name, "integer"); + return result; + } + + if (!descriptor.floating_point_range.empty() && value.get_type() == rclcpp::PARAMETER_DOUBLE) { + double v = value.get(); + auto fp_range = descriptor.floating_point_range.at(0); + if (__are_doubles_equal(v, fp_range.from_value) || __are_doubles_equal(v, fp_range.to_value)) { + return result; + } + if ((v < fp_range.from_value) || (v > fp_range.to_value)) { + result.successful = false; + format_reason(result.reason, descriptor.name, "floating point"); + return result; + } + if (fp_range.step == 0.0) { + return result; + } + double rounded_div = std::round((v - fp_range.from_value) / fp_range.step); + if (__are_doubles_equal(v, fp_range.from_value + rounded_div * fp_range.step)) { + return result; + } + result.successful = false; + format_reason(result.reason, descriptor.name, "floating point"); + return result; + } + return result; +} + +// Return true if parameter values comply with the descriptors in parameter_infos. +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__check_parameters( + std::map & parameter_infos, + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const rclcpp::Parameter & parameter : parameters) { + const rcl_interfaces::msg::ParameterDescriptor & descriptor = + parameter_infos[parameter.get_name()].descriptor; + result = __check_parameter_value_in_range( + descriptor, + parameter.get_parameter_value()); + if (!result.successful) { + break; + } + } + return result; +} + +using OnParametersSetCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; +using CallbacksContainerType = + rclcpp::node_interfaces::NodeParameters::CallbacksContainerType; +using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__call_on_parameters_set_callbacks( + const std::vector & parameters, + CallbacksContainerType & callback_container, + const OnParametersSetCallbackType & callback) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + result = shared_handle->callback(parameters); + if (!result.successful) { + return result; + } + it++; + } else { + it = callback_container.erase(it); + } + } + if (callback) { + result = callback(parameters); + } + return result; +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__set_parameters_atomically_common( + const std::vector & parameters, + std::map & parameter_infos, + CallbacksContainerType & callback_container, + const OnParametersSetCallbackType & callback) +{ + // Call the user callback to see if the new value(s) are allowed. + rcl_interfaces::msg::SetParametersResult result = + __call_on_parameters_set_callbacks(parameters, callback_container, callback); + if (!result.successful) { + return result; + } + // Check if the value being set complies with the descriptor. + result = __check_parameters(parameter_infos, parameters); + if (!result.successful) { + return result; + } + // If accepted, actually set the values. + if (result.successful) { + for (size_t i = 0; i < parameters.size(); ++i) { + const std::string & name = parameters[i].get_name(); + parameter_infos[name].descriptor.name = parameters[i].get_name(); + parameter_infos[name].descriptor.type = parameters[i].get_type(); + parameter_infos[name].value = parameters[i].get_parameter_value(); + } + } + + // Either way, return the result. + return result; +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__declare_parameter_common( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + std::map & parameters_out, + const std::map & overrides, + CallbacksContainerType & callback_container, + const OnParametersSetCallbackType & callback, + rcl_interfaces::msg::ParameterEvent * parameter_event_out, + bool ignore_override = false) +{ + using rclcpp::node_interfaces::ParameterInfo; + std::map parameter_infos {{name, ParameterInfo()}}; + parameter_infos.at(name).descriptor = parameter_descriptor; + + // Use the value from the overrides if available, otherwise use the default. + const rclcpp::ParameterValue * initial_value = &default_value; + auto overrides_it = overrides.find(name); + if (!ignore_override && overrides_it != overrides.end()) { + initial_value = &overrides_it->second; + } + + // Check with the user's callback to see if the initial value can be set. + std::vector parameter_wrappers {rclcpp::Parameter(name, *initial_value)}; + // This function also takes care of default vs initial value. + auto result = __set_parameters_atomically_common( + parameter_wrappers, + parameter_infos, + callback_container, + callback); + + // Add declared parameters to storage. + parameters_out[name] = parameter_infos.at(name); + + // Extend the given parameter event, if valid. + if (parameter_event_out) { + parameter_event_out->new_parameters.push_back(parameter_wrappers[0].to_parameter_msg()); + } + + return result; +} + +const rclcpp::ParameterValue & +NodeParameters::declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + std::lock_guard lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + // TODO(sloretz) parameter name validation + if (name.empty()) { + throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty"); + } + + // Error if this parameter has already been declared and is different + if (__lockless_has_parameter(parameters_, name)) { + throw rclcpp::exceptions::ParameterAlreadyDeclaredException( + "parameter '" + name + "' has already been declared"); + } + + rcl_interfaces::msg::ParameterEvent parameter_event; + auto result = __declare_parameter_common( + name, + default_value, + parameter_descriptor, + parameters_, + parameter_overrides_, + on_parameters_set_callback_container_, + on_parameters_set_callback_, + ¶meter_event, + ignore_override); + + // If it failed to be set, then throw an exception. + if (!result.successful) { + throw rclcpp::exceptions::InvalidParameterValueException( + "parameter '" + name + "' could not be set: " + result.reason); + } + + // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor. + if (nullptr != events_publisher_) { + parameter_event.node = combined_name_; + parameter_event.stamp = node_clock_->get_clock()->now(); + events_publisher_->publish(parameter_event); + } + + return parameters_.at(name).value; +} + +void +NodeParameters::undeclare_parameter(const std::string & name) +{ + std::lock_guard lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto parameter_info = parameters_.find(name); + if (parameter_info == parameters_.end()) { + throw rclcpp::exceptions::ParameterNotDeclaredException( + "cannot undeclare parameter '" + name + "' which has not yet been declared"); + } + + if (parameter_info->second.descriptor.read_only) { + throw rclcpp::exceptions::ParameterImmutableException( + "cannot undeclare parameter '" + name + "' because it is read-only"); + } + + parameters_.erase(parameter_info); +} + +bool +NodeParameters::has_parameter(const std::string & name) const +{ + std::lock_guard lock(mutex_); + + return __lockless_has_parameter(parameters_, name); +} + +std::vector +NodeParameters::set_parameters(const std::vector & parameters) +{ + std::vector results; + results.reserve(parameters.size()); + + for (const auto & p : parameters) { + auto result = set_parameters_atomically({{p}}); + results.push_back(result); + } + + return results; +} + +template +auto +__find_parameter_by_name( + ParameterVectorType & parameters, + const std::string & name) +{ + return std::find_if( + parameters.begin(), + parameters.end(), + [&](auto parameter) {return parameter.get_name() == name;}); +} + +rcl_interfaces::msg::SetParametersResult +NodeParameters::set_parameters_atomically(const std::vector & parameters) +{ + std::lock_guard lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + rcl_interfaces::msg::SetParametersResult result; + + // Check if any of the parameters are read-only, or if any parameters are not + // declared. + // If not declared, keep track of them in order to declare them later, when + // undeclared parameters are allowed, and if they're not allowed, fail. + std::vector parameters_to_be_declared; + for (const auto & parameter : parameters) { + const std::string & name = parameter.get_name(); + + // Check to make sure the parameter name is valid. + if (name.empty()) { + throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty"); + } + + // Check to see if it is declared. + auto parameter_info = parameters_.find(name); + if (parameter_info == parameters_.end()) { + // If not check to see if undeclared paramaters are allowed, ... + if (allow_undeclared_) { + // If so, mark the parameter to be declared for the user implicitly. + parameters_to_be_declared.push_back(¶meter); + // continue as it cannot be read-only, and because the declare will + // implicitly set the parameter and parameter_infos is for setting only. + continue; + } else { + // If not, then throw the exception as documented. + throw rclcpp::exceptions::ParameterNotDeclaredException( + "parameter '" + name + "' cannot be set because it was not declared"); + } + } + + // Check to see if it is read-only. + if (parameter_info->second.descriptor.read_only) { + result.successful = false; + result.reason = "parameter '" + name + "' cannot be set because it is read-only"; + return result; + } + } + + // Declare parameters into a temporary "staging area", incase one of the declares fail. + // We will use the staged changes as input to the "set atomically" action. + // We explicitly avoid calling the user callback here, so that it may be called once, with + // all the other parameters to be set (already declared parameters). + std::map staged_parameter_changes; + rcl_interfaces::msg::ParameterEvent parameter_event_msg; + parameter_event_msg.node = combined_name_; + CallbacksContainerType empty_callback_container; + for (auto parameter_to_be_declared : parameters_to_be_declared) { + // This should not throw, because we validated the name and checked that + // the parameter was not already declared. + result = __declare_parameter_common( + parameter_to_be_declared->get_name(), + parameter_to_be_declared->get_parameter_value(), + rcl_interfaces::msg::ParameterDescriptor(), // Implicit declare uses default descriptor. + staged_parameter_changes, + parameter_overrides_, + // Only call callbacks once below + empty_callback_container, // callback_container is explicitly empty + nullptr, // callback is explicitly null. + ¶meter_event_msg, + true); + if (!result.successful) { + // Declare failed, return knowing that nothing was changed because the + // staged changes were not applied. + return result; + } + } + + // If there were implicitly declared parameters, then we may need to copy the input parameters + // and then assign the value that was selected after the declare (could be affected by the + // initial parameter values). + const std::vector * parameters_to_be_set = ¶meters; + std::vector parameters_copy; + if (0 != staged_parameter_changes.size()) { // If there were any implicitly declared parameters. + bool any_initial_values_used = false; + for (const auto & staged_parameter_change : staged_parameter_changes) { + auto it = __find_parameter_by_name(parameters, staged_parameter_change.first); + if (it->get_parameter_value() != staged_parameter_change.second.value) { + // In this case, the value of the staged parameter differs from the + // input from the user, and therefore we need to update things before setting. + any_initial_values_used = true; + // No need to search further since at least one initial value needs to be used. + break; + } + } + if (any_initial_values_used) { + parameters_copy = parameters; + for (const auto & staged_parameter_change : staged_parameter_changes) { + auto it = __find_parameter_by_name(parameters_copy, staged_parameter_change.first); + *it = Parameter(staged_parameter_change.first, staged_parameter_change.second.value); + } + parameters_to_be_set = ¶meters_copy; + } + } + + // Collect parameters who will have had their type changed to + // rclcpp::PARAMETER_NOT_SET so they can later be implicitly undeclared. + std::vector parameters_to_be_undeclared; + for (const auto & parameter : *parameters_to_be_set) { + if (rclcpp::PARAMETER_NOT_SET == parameter.get_type()) { + auto it = parameters_.find(parameter.get_name()); + if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) { + parameters_to_be_undeclared.push_back(¶meter); + } + } + } + + // Set all of the parameters including the ones declared implicitly above. + result = __set_parameters_atomically_common( + // either the original parameters given by the user, or ones updated with initial values + *parameters_to_be_set, + // they are actually set on the official parameter storage + parameters_, + // this will get called once, with all the parameters to be set + on_parameters_set_callback_container_, + // These callbacks are called once. When a callback returns an unsuccessful result, + // the remaining aren't called. + on_parameters_set_callback_); + + // If not successful, then stop here. + if (!result.successful) { + return result; + } + + // If successful, then update the parameter infos from the implicitly declared parameter's. + for (const auto & kv_pair : staged_parameter_changes) { + // assumption: the parameter is already present in parameters_ due to the above "set" + assert(__lockless_has_parameter(parameters_, kv_pair.first)); + // assumption: the value in parameters_ is the same as the value resulting from the declare + assert(parameters_[kv_pair.first].value == kv_pair.second.value); + // This assignment should not change the name, type, or value, but may + // change other things from the ParameterInfo. + parameters_[kv_pair.first] = kv_pair.second; + } + + // Undeclare parameters that need to be. + for (auto parameter_to_undeclare : parameters_to_be_undeclared) { + auto it = parameters_.find(parameter_to_undeclare->get_name()); + // assumption: the parameter to be undeclared should be in the parameter infos map + assert(it != parameters_.end()); + if (it != parameters_.end()) { + // Update the parameter event message and remove it. + parameter_event_msg.deleted_parameters.push_back( + rclcpp::Parameter(it->first, it->second.value).to_parameter_msg()); + parameters_.erase(it); + } + } + + // Update the parameter event message for any parameters which were only set, + // and not either declared or undeclared. + for (const auto & parameter : *parameters_to_be_set) { + if (staged_parameter_changes.find(parameter.get_name()) != staged_parameter_changes.end()) { + // This parameter was declared. + continue; + } + auto it = std::find_if( + parameters_to_be_undeclared.begin(), + parameters_to_be_undeclared.end(), + [¶meter](const auto & p) {return p->get_name() == parameter.get_name();}); + if (it != parameters_to_be_undeclared.end()) { + // This parameter was undeclared (deleted). + continue; + } + // This parameter was neither declared nor undeclared. + parameter_event_msg.changed_parameters.push_back(parameter.to_parameter_msg()); + } + + // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor. + if (nullptr != events_publisher_) { + parameter_event_msg.stamp = node_clock_->get_clock()->now(); + events_publisher_->publish(parameter_event_msg); + } + + return result; +} + +std::vector +NodeParameters::get_parameters(const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + results.reserve(names.size()); + + for (auto & name : names) { + auto found_parameter = parameters_.find(name); + if (found_parameter != parameters_.cend()) { + // found + results.emplace_back(name, found_parameter->second.value); + } else if (this->allow_undeclared_) { + // not found, but undeclared allowed + results.emplace_back(name, rclcpp::ParameterValue()); + } else { + // not found, and undeclared are not allowed + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } + } + return results; +} + +rclcpp::Parameter +NodeParameters::get_parameter(const std::string & name) const +{ + rclcpp::Parameter parameter; + + if (get_parameter(name, parameter)) { + return parameter; + } else if (this->allow_undeclared_) { + return parameter; + } else { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } +} + +bool +NodeParameters::get_parameter( + const std::string & name, + rclcpp::Parameter & parameter) const +{ + std::lock_guard lock(mutex_); + + auto param_iter = parameters_.find(name); + if ( + parameters_.end() != param_iter && + param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) + { + parameter = {name, param_iter->second.value}; + return true; + } else { + return false; + } +} + +bool +NodeParameters::get_parameters_by_prefix( + const std::string & prefix, + std::map & parameters) const +{ + std::lock_guard lock(mutex_); + + std::string prefix_with_dot = prefix.empty() ? prefix : prefix + "."; + bool ret = false; + + for (const auto & param : parameters_) { + if (param.first.find(prefix_with_dot) == 0 && param.first.length() > prefix_with_dot.length()) { + // Found one! + parameters[param.first.substr(prefix_with_dot.length())] = rclcpp::Parameter(param.second); + ret = true; + } + } + + return ret; +} + +std::vector +NodeParameters::describe_parameters(const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + results.reserve(names.size()); + + for (const auto & name : names) { + auto it = parameters_.find(name); + if (it != parameters_.cend()) { + results.push_back(it->second.descriptor); + } else if (allow_undeclared_) { + // parameter not found, but undeclared allowed, so return empty + rcl_interfaces::msg::ParameterDescriptor default_description; + default_description.name = name; + results.push_back(default_description); + } else { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } + } + + if (results.size() != names.size()) { + throw std::runtime_error("results and names unexpectedly different sizes"); + } + + return results; +} + +std::vector +NodeParameters::get_parameter_types(const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + results.reserve(names.size()); + + for (const auto & name : names) { + auto it = parameters_.find(name); + if (it != parameters_.cend()) { + results.push_back(it->second.value.get_type()); + } else if (allow_undeclared_) { + // parameter not found, but undeclared allowed, so return not set + results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } else { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } + } + + if (results.size() != names.size()) { + throw std::runtime_error("results and names unexpectedly different sizes"); + } + + return results; +} + +rcl_interfaces::msg::ListParametersResult +NodeParameters::list_parameters(const std::vector & prefixes, uint64_t depth) const +{ + std::lock_guard lock(mutex_); + rcl_interfaces::msg::ListParametersResult result; + + // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity + // using "." for now + const char * separator = "."; + for (auto & kv : parameters_) { + bool get_all = (prefixes.size() == 0) && + ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || + (static_cast(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth)); + bool prefix_matches = std::any_of( + prefixes.cbegin(), prefixes.cend(), + [&kv, &depth, &separator](const std::string & prefix) { + if (kv.first == prefix) { + return true; + } else if (kv.first.find(prefix + separator) == 0) { + size_t length = prefix.length(); + std::string substr = kv.first.substr(length); + // Cast as unsigned integer to avoid warning + return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || + (static_cast(std::count(substr.begin(), substr.end(), *separator)) < depth); + } + return false; + }); + if (get_all || prefix_matches) { + result.names.push_back(kv.first); + size_t last_separator = kv.first.find_last_of(separator); + if (std::string::npos != last_separator) { + std::string prefix = kv.first.substr(0, last_separator); + if ( + std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == + result.prefixes.cend()) + { + result.prefixes.push_back(prefix); + } + } + } + } + return result; +} + +struct HandleCompare + : public std::unary_function +{ + explicit HandleCompare(const OnSetParametersCallbackHandle * const base) + : base_(base) {} + bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle) + { + auto shared_handle = handle.lock(); + if (base_ == shared_handle.get()) { + return true; + } + return false; + } + const OnSetParametersCallbackHandle * const base_; +}; + +void +NodeParameters::remove_on_set_parameters_callback( + const OnSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + on_parameters_set_callback_container_.begin(), + on_parameters_set_callback_container_.end(), + HandleCompare(handle)); + if (it != on_parameters_set_callback_container_.end()) { + on_parameters_set_callback_container_.erase(it); + } else { + throw std::runtime_error("Callback doesn't exist"); + } +} + +OnSetParametersCallbackHandle::SharedPtr +NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + on_parameters_set_callback_container_.emplace_front(handle); + return handle; +} + +NodeParameters::OnParametersSetCallbackType +NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback) +{ + std::lock_guard lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto existing_callback = on_parameters_set_callback_; + on_parameters_set_callback_ = callback; + return existing_callback; +} + +const std::map & +NodeParameters::get_parameter_overrides() const +{ + return parameter_overrides_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_services.cpp new file mode 100644 index 0000000..0e74f58 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -0,0 +1,80 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/node_interfaces/node_services.hpp" + +#include + +using rclcpp::node_interfaces::NodeServices; + +NodeServices::NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeServices::~NodeServices() +{} + +void +NodeServices::add_service( + rclcpp::ServiceBase::SharedPtr service_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create service, group not in node."); + } + group->add_service(service_base_ptr); + } else { + node_base_->get_default_callback_group()->add_service(service_base_ptr); + } + + // Notify the executor that a new service was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify wait set on service creation: ") + + rmw_get_error_string().str + ); + } + } +} + +void +NodeServices::add_client( + rclcpp::ClientBase::SharedPtr client_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create client, group not in node."); + } + group->add_client(client_base_ptr); + } else { + node_base_->get_default_callback_group()->add_client(client_base_ptr); + } + + // Notify the executor that a new client was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify wait set on client creation: ") + + rmw_get_error_string().str + ); + } + } +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp new file mode 100644 index 0000000..7c678b7 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp @@ -0,0 +1,50 @@ +// Copyright 2018 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 "rclcpp/node_interfaces/node_time_source.hpp" + +#include +#include + +using rclcpp::node_interfaces::NodeTimeSource; + +NodeTimeSource::NodeTimeSource( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters) +: node_base_(node_base), + node_topics_(node_topics), + node_graph_(node_graph), + node_services_(node_services), + node_logging_(node_logging), + node_clock_(node_clock), + node_parameters_(node_parameters) +{ + time_source_.attachNode( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_); + time_source_.attachClock(node_clock_->get_clock()); +} + +NodeTimeSource::~NodeTimeSource() +{} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp new file mode 100644 index 0000000..f9d5d72 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -0,0 +1,47 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/node_interfaces/node_timers.hpp" + +#include + +using rclcpp::node_interfaces::NodeTimers; + +NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeTimers::~NodeTimers() +{} + +void +NodeTimers::add_timer( + rclcpp::TimerBase::SharedPtr timer, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) +{ + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create timer, group not in node."); + } + callback_group->add_timer(timer); + } else { + node_base_->get_default_callback_group()->add_timer(timer); + } + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify wait set on timer creation: ") + + rmw_get_error_string().str); + } +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp new file mode 100644 index 0000000..11de4a3 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -0,0 +1,123 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/node_interfaces/node_topics.hpp" + +#include + +#include "rclcpp/exceptions.hpp" + +using rclcpp::exceptions::throw_from_rcl_error; + +using rclcpp::node_interfaces::NodeTopics; + +NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeTopics::~NodeTopics() +{} + +rclcpp::PublisherBase::SharedPtr +NodeTopics::create_publisher( + const std::string & topic_name, + const rclcpp::PublisherFactory & publisher_factory, + const rclcpp::QoS & qos) +{ + // Create the MessageT specific Publisher using the factory, but return it as PublisherBase. + return publisher_factory.create_typed_publisher(node_base_, topic_name, qos); +} + +void +NodeTopics::add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) +{ + // Assign to a group. + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + throw std::runtime_error("Cannot create publisher, callback group not in node."); + } + } else { + callback_group = node_base_->get_default_callback_group(); + } + + for (auto & publisher_event : publisher->get_event_handlers()) { + callback_group->add_waitable(publisher_event); + } + + // Notify the executor that a new publisher was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify wait set on publisher creation: ") + + rmw_get_error_string().str); + } + } +} + +rclcpp::SubscriptionBase::SharedPtr +NodeTopics::create_subscription( + const std::string & topic_name, + const rclcpp::SubscriptionFactory & subscription_factory, + const rclcpp::QoS & qos) +{ + // Create the MessageT specific Subscription using the factory, but return a SubscriptionBase. + return subscription_factory.create_typed_subscription(node_base_, topic_name, qos); +} + +void +NodeTopics::add_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) +{ + // Assign to a group. + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create subscription, callback group not in node."); + } + } else { + callback_group = node_base_->get_default_callback_group(); + } + + callback_group->add_subscription(subscription); + + for (auto & subscription_event : subscription->get_event_handlers()) { + callback_group->add_waitable(subscription_event); + } + + auto intra_process_waitable = subscription->get_intra_process_waitable(); + if (nullptr != intra_process_waitable) { + // Add to the callback group to be notified about intra-process msgs. + callback_group->add_waitable(intra_process_waitable); + } + + // Notify the executor that a new subscription was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); + if (ret != RCL_RET_OK) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to notify wait set on subscription creation"); + } + } +} + +rclcpp::node_interfaces::NodeBaseInterface * +NodeTopics::get_node_base_interface() const +{ + return node_base_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp new file mode 100644 index 0000000..ee9a2da --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -0,0 +1,68 @@ +// Copyright 2018 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 "rclcpp/node_interfaces/node_waitables.hpp" + +#include + +using rclcpp::node_interfaces::NodeWaitables; + +NodeWaitables::NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeWaitables::~NodeWaitables() +{} + +void +NodeWaitables::add_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + // TODO(jacobperron): use custom exception + throw std::runtime_error("Cannot create waitable, group not in node."); + } + group->add_waitable(waitable_ptr); + } else { + node_base_->get_default_callback_group()->add_waitable(waitable_ptr); + } + + // Notify the executor that a new waitable was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify wait set on waitable creation: ") + + rmw_get_error_string().str + ); + } + } +} + +void +NodeWaitables::remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + return; + } + group->remove_waitable(waitable_ptr); + } else { + node_base_->get_default_callback_group()->remove_waitable(waitable_ptr); + } +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/node_options.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/node_options.cpp new file mode 100644 index 0000000..65fe322 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/node_options.cpp @@ -0,0 +1,341 @@ +// Copyright 2019 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 "rclcpp/node_options.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" + +using rclcpp::exceptions::throw_from_rcl_error; + +namespace rclcpp +{ + +namespace detail +{ +static +void +rcl_node_options_t_destructor(rcl_node_options_t * node_options) +{ + if (node_options) { + rcl_ret_t ret = rcl_node_options_fini(node_options); + if (RCL_RET_OK != ret) { + // Cannot throw here, as it may be called in the destructor. + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize rcl node options: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + + delete node_options; + node_options = nullptr; + } +} +} // namespace detail + +NodeOptions::NodeOptions(rcl_allocator_t allocator) +: node_options_(nullptr, detail::rcl_node_options_t_destructor), allocator_(allocator) +{} + +NodeOptions::NodeOptions(const NodeOptions & other) +: node_options_(nullptr, detail::rcl_node_options_t_destructor) +{ + *this = other; +} + +NodeOptions & +NodeOptions::operator=(const NodeOptions & other) +{ + if (this != &other) { + this->context_ = other.context_; + this->arguments_ = other.arguments_; + this->parameter_overrides_ = other.parameter_overrides_; + this->use_global_arguments_ = other.use_global_arguments_; + this->use_intra_process_comms_ = other.use_intra_process_comms_; + this->start_parameter_services_ = other.start_parameter_services_; + this->allocator_ = other.allocator_; + this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_; + this->automatically_declare_parameters_from_overrides_ = + other.automatically_declare_parameters_from_overrides_; + } + return *this; +} + +const rcl_node_options_t * +NodeOptions::get_rcl_node_options() const +{ + // If it is nullptr, create it on demand. + if (!node_options_) { + node_options_.reset(new rcl_node_options_t); + *node_options_ = rcl_node_get_default_options(); + node_options_->allocator = this->allocator_; + node_options_->use_global_arguments = this->use_global_arguments_; + node_options_->domain_id = this->get_domain_id_from_env(); + + int c_argc = 0; + std::unique_ptr c_argv; + if (!this->arguments_.empty()) { + if (this->arguments_.size() > static_cast(std::numeric_limits::max())) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args"); + } + + c_argc = static_cast(this->arguments_.size()); + c_argv.reset(new const char *[c_argc]); + + for (std::size_t i = 0; i < this->arguments_.size(); ++i) { + c_argv[i] = this->arguments_[i].c_str(); + } + } + + rcl_ret_t ret = rcl_parse_arguments( + c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments)); + + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to parse arguments"); + } + + int unparsed_ros_args_count = + rcl_arguments_get_count_unparsed_ros(&(node_options_->arguments)); + if (unparsed_ros_args_count > 0) { + int * unparsed_ros_args_indices = nullptr; + ret = rcl_arguments_get_unparsed_ros( + &(node_options_->arguments), this->allocator_, &unparsed_ros_args_indices); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to get unparsed ROS arguments"); + } + try { + std::vector unparsed_ros_args; + for (int i = 0; i < unparsed_ros_args_count; ++i) { + unparsed_ros_args.push_back(c_argv[unparsed_ros_args_indices[i]]); + } + throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_args)); + } catch (...) { + this->allocator_.deallocate(unparsed_ros_args_indices, this->allocator_.state); + throw; + } + } + } + + return node_options_.get(); +} + +rclcpp::Context::SharedPtr +NodeOptions::context() const +{ + return this->context_; +} + +NodeOptions & +NodeOptions::context(rclcpp::Context::SharedPtr context) +{ + this->context_ = context; + return *this; +} + +const std::vector & +NodeOptions::arguments() const +{ + return this->arguments_; +} + +NodeOptions & +NodeOptions::arguments(const std::vector & arguments) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->arguments_ = arguments; + return *this; +} + +std::vector & +NodeOptions::parameter_overrides() +{ + return this->parameter_overrides_; +} + +const std::vector & +NodeOptions::parameter_overrides() const +{ + return this->parameter_overrides_; +} + +NodeOptions & +NodeOptions::parameter_overrides(const std::vector & parameter_overrides) +{ + this->parameter_overrides_ = parameter_overrides; + return *this; +} + +bool +NodeOptions::use_global_arguments() const +{ + return this->node_options_->use_global_arguments; +} + +NodeOptions & +NodeOptions::use_global_arguments(bool use_global_arguments) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->use_global_arguments_ = use_global_arguments; + return *this; +} + +bool +NodeOptions::use_intra_process_comms() const +{ + return this->use_intra_process_comms_; +} + +NodeOptions & +NodeOptions::use_intra_process_comms(bool use_intra_process_comms) +{ + this->use_intra_process_comms_ = use_intra_process_comms; + return *this; +} + +bool +NodeOptions::start_parameter_services() const +{ + return this->start_parameter_services_; +} + +NodeOptions & +NodeOptions::start_parameter_services(bool start_parameter_services) +{ + this->start_parameter_services_ = start_parameter_services; + return *this; +} + +bool +NodeOptions::start_parameter_event_publisher() const +{ + return this->start_parameter_event_publisher_; +} + +NodeOptions & +NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publisher) +{ + this->start_parameter_event_publisher_ = start_parameter_event_publisher; + return *this; +} + +const rclcpp::QoS & +NodeOptions::parameter_event_qos() const +{ + return this->parameter_event_qos_; +} + +NodeOptions & +NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos) +{ + this->parameter_event_qos_ = parameter_event_qos; + return *this; +} + +const rclcpp::PublisherOptionsBase & +NodeOptions::parameter_event_publisher_options() const +{ + return parameter_event_publisher_options_; +} + +NodeOptions & +NodeOptions::parameter_event_publisher_options( + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options) +{ + parameter_event_publisher_options_ = parameter_event_publisher_options; + return *this; +} + +bool +NodeOptions::allow_undeclared_parameters() const +{ + return this->allow_undeclared_parameters_; +} + +NodeOptions & +NodeOptions::allow_undeclared_parameters(bool allow_undeclared_parameters) +{ + this->allow_undeclared_parameters_ = allow_undeclared_parameters; + return *this; +} + +bool +NodeOptions::automatically_declare_parameters_from_overrides() const +{ + return this->automatically_declare_parameters_from_overrides_; +} + +NodeOptions & +NodeOptions::automatically_declare_parameters_from_overrides( + bool automatically_declare_parameters_from_overrides) +{ + this->automatically_declare_parameters_from_overrides_ = + automatically_declare_parameters_from_overrides; + return *this; +} + +const rcl_allocator_t & +NodeOptions::allocator() const +{ + return this->allocator_; +} + +NodeOptions & +NodeOptions::allocator(rcl_allocator_t allocator) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->allocator_ = allocator; + return *this; +} + +// TODO(wjwwood): reuse rcutils_get_env() to avoid code duplication. +// See also: https://github.com/ros2/rcl/issues/119 +size_t +NodeOptions::get_domain_id_from_env() const +{ + // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable. + size_t domain_id = std::numeric_limits::max(); + char * ros_domain_id = nullptr; + const char * env_var = "ROS_DOMAIN_ID"; +#ifndef _WIN32 + ros_domain_id = getenv(env_var); +#else + size_t ros_domain_id_size; + _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); +#endif + if (ros_domain_id) { + uint32_t number = strtoul(ros_domain_id, NULL, 0); + if (number == (std::numeric_limits::max)()) { +#ifdef _WIN32 + // free the ros_domain_id before throwing, if getenv was used on Windows + free(ros_domain_id); +#endif + throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); + } + domain_id = static_cast(number); +#ifdef _WIN32 + free(ros_domain_id); +#endif + } + return domain_id; +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/parameter.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter.cpp new file mode 100644 index 0000000..673e06a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter.cpp @@ -0,0 +1,215 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/parameter.hpp" + +#include +#include +#include +#include + +#include "rclcpp/node_interfaces/node_parameters.hpp" +#include "rclcpp/utilities.hpp" + +using rclcpp::ParameterType; +using rclcpp::Parameter; + +Parameter::Parameter() +: name_("") +{ +} + +Parameter::Parameter(const std::string & name) +: name_(name), value_() +{ +} + +Parameter::Parameter(const std::string & name, const rclcpp::ParameterValue & value) +: name_(name), value_(value) +{ +} + +Parameter::Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info) +: Parameter(parameter_info.descriptor.name, parameter_info.value) +{ +} + +bool +Parameter::operator==(const Parameter & rhs) const +{ + return this->name_ == rhs.name_ && this->value_ == rhs.value_; +} + +bool +Parameter::operator!=(const Parameter & rhs) const +{ + return !(*this == rhs); +} + +ParameterType +Parameter::get_type() const +{ + return value_.get_type(); +} + +std::string +Parameter::get_type_name() const +{ + return rclcpp::to_string(get_type()); +} + +const std::string & +Parameter::get_name() const +{ + return name_; +} + +rcl_interfaces::msg::ParameterValue +Parameter::get_value_message() const +{ + return value_.to_value_msg(); +} + +const rclcpp::ParameterValue & +Parameter::get_parameter_value() const +{ + return value_; +} + +bool +Parameter::as_bool() const +{ + return get_value(); +} + +int64_t +Parameter::as_int() const +{ + return get_value(); +} + +double +Parameter::as_double() const +{ + return get_value(); +} + +const std::string & +Parameter::as_string() const +{ + return get_value(); +} + +const std::vector & +Parameter::as_byte_array() const +{ + return get_value(); +} + +const std::vector & +Parameter::as_bool_array() const +{ + return get_value(); +} + +const std::vector & +Parameter::as_integer_array() const +{ + return get_value(); +} + +const std::vector & +Parameter::as_double_array() const +{ + return get_value(); +} + +const std::vector & +Parameter::as_string_array() const +{ + return get_value(); +} + +Parameter +Parameter::from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter) +{ + return Parameter(parameter.name, parameter.value); +} + +rcl_interfaces::msg::Parameter +Parameter::to_parameter_msg() const +{ + rcl_interfaces::msg::Parameter parameter; + parameter.name = name_; + parameter.value = value_.to_value_msg(); + return parameter; +} + +std::string +Parameter::value_to_string() const +{ + return rclcpp::to_string(value_); +} + +std::string +rclcpp::_to_json_dict_entry(const Parameter & param) +{ + std::stringstream ss; + ss << "\"" << param.get_name() << "\": "; + ss << "{\"type\": \"" << param.get_type_name() << "\", "; + ss << "\"value\": \"" << param.value_to_string() << "\"}"; + return ss.str(); +} + +std::ostream & +rclcpp::operator<<(std::ostream & os, const rclcpp::Parameter & pv) +{ + os << std::to_string(pv); + return os; +} + +std::ostream & +rclcpp::operator<<(std::ostream & os, const std::vector & parameters) +{ + os << std::to_string(parameters); + return os; +} + +std::string +std::to_string(const rclcpp::Parameter & param) +{ + std::stringstream ss; + ss << "{\"name\": \"" << param.get_name() << "\", "; + ss << "\"type\": \"" << param.get_type_name() << "\", "; + ss << "\"value\": \"" << param.value_to_string() << "\"}"; + return ss.str(); +} + +std::string +std::to_string(const std::vector & parameters) +{ + std::stringstream ss; + ss << "{"; + bool first = true; + for (const auto & pv : parameters) { + if (first == false) { + ss << ", "; + } else { + first = false; + } + ss << rclcpp::_to_json_dict_entry(pv); + } + ss << "}"; + return ss.str(); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_client.cpp new file mode 100644 index 0000000..8afbf43 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_client.cpp @@ -0,0 +1,505 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/parameter_client.hpp" + +#include +#include +#include +#include + +#include "./parameter_service_names.hpp" + +using rclcpp::AsyncParametersClient; +using rclcpp::SyncParametersClient; + +AsyncParametersClient::AsyncParametersClient( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +: node_topics_interface_(node_topics_interface) +{ + if (remote_node_name != "") { + remote_node_name_ = remote_node_name; + } else { + remote_node_name_ = node_base_interface->get_name(); + } + + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos_profile; + + using rclcpp::Client; + using rclcpp::ClientBase; + + get_parameters_client_ = Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::get_parameters, + options); + auto get_parameters_base = std::dynamic_pointer_cast(get_parameters_client_); + node_services_interface->add_client(get_parameters_base, group); + + get_parameter_types_client_ = Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::get_parameter_types, + options); + auto get_parameter_types_base = + std::dynamic_pointer_cast(get_parameter_types_client_); + node_services_interface->add_client(get_parameter_types_base, group); + + set_parameters_client_ = Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::set_parameters, + options); + auto set_parameters_base = std::dynamic_pointer_cast(set_parameters_client_); + node_services_interface->add_client(set_parameters_base, group); + + set_parameters_atomically_client_ = + Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, + options); + auto set_parameters_atomically_base = std::dynamic_pointer_cast( + set_parameters_atomically_client_); + node_services_interface->add_client(set_parameters_atomically_base, group); + + list_parameters_client_ = Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::list_parameters, + options); + auto list_parameters_base = std::dynamic_pointer_cast(list_parameters_client_); + node_services_interface->add_client(list_parameters_base, group); + + describe_parameters_client_ = Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::describe_parameters, + options); + auto describe_parameters_base = + std::dynamic_pointer_cast(describe_parameters_client_); + node_services_interface->add_client(describe_parameters_base, group); +} + +AsyncParametersClient::AsyncParametersClient( + const rclcpp::Node::SharedPtr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +: AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile, + group) +{} + +AsyncParametersClient::AsyncParametersClient( + rclcpp::Node * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +: AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile, + group) +{} + +std::shared_future> +AsyncParametersClient::get_parameters( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback) +{ + auto promise_result = + std::make_shared>>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + request->names = names; + + get_parameters_client_->async_send_request( + request, + [request, promise_result, future_result, callback]( + rclcpp::Client::SharedFuture cb_f) + { + std::vector parameters; + auto & pvalues = cb_f.get()->values; + + for (auto & pvalue : pvalues) { + auto i = &pvalue - &pvalues[0]; + rcl_interfaces::msg::Parameter parameter; + parameter.name = request->names[i]; + parameter.value = pvalue; + parameters.push_back(rclcpp::Parameter::from_parameter_msg(parameter)); + } + + promise_result->set_value(parameters); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; +} + +std::shared_future> +AsyncParametersClient::get_parameter_types( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback) +{ + auto promise_result = + std::make_shared>>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + request->names = names; + + get_parameter_types_client_->async_send_request( + request, + [promise_result, future_result, callback]( + rclcpp::Client::SharedFuture cb_f) + { + std::vector types; + auto & pts = cb_f.get()->types; + for (auto & pt : pts) { + pts.push_back(static_cast(pt)); + } + promise_result->set_value(types); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; +} + +std::shared_future> +AsyncParametersClient::set_parameters( + const std::vector & parameters, + std::function< + void(std::shared_future>) + > callback) +{ + auto promise_result = + std::make_shared>>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::Parameter p) {return p.to_parameter_msg();} + ); + + set_parameters_client_->async_send_request( + request, + [promise_result, future_result, callback]( + rclcpp::Client::SharedFuture cb_f) + { + promise_result->set_value(cb_f.get()->results); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; +} + +std::shared_future +AsyncParametersClient::set_parameters_atomically( + const std::vector & parameters, + std::function< + void(std::shared_future) + > callback) +{ + auto promise_result = + std::make_shared>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::Parameter p) {return p.to_parameter_msg();} + ); + + set_parameters_atomically_client_->async_send_request( + request, + [promise_result, future_result, callback]( + rclcpp::Client::SharedFuture cb_f) + { + promise_result->set_value(cb_f.get()->result); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; +} + +std::shared_future +AsyncParametersClient::list_parameters( + const std::vector & prefixes, + uint64_t depth, + std::function< + void(std::shared_future) + > callback) +{ + auto promise_result = + std::make_shared>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + request->prefixes = prefixes; + request->depth = depth; + + list_parameters_client_->async_send_request( + request, + [promise_result, future_result, callback]( + rclcpp::Client::SharedFuture cb_f) + { + promise_result->set_value(cb_f.get()->result); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; +} + +bool +AsyncParametersClient::service_is_ready() const +{ + return + get_parameters_client_->service_is_ready() && + get_parameter_types_client_->service_is_ready() && + set_parameters_client_->service_is_ready() && + list_parameters_client_->service_is_ready() && + describe_parameters_client_->service_is_ready(); +} + +bool +AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) +{ + const std::vector> clients = { + get_parameters_client_, + get_parameter_types_client_, + set_parameters_client_, + list_parameters_client_, + describe_parameters_client_ + }; + for (auto & client : clients) { + auto stamp = std::chrono::steady_clock::now(); + if (!client->wait_for_service(timeout)) { + return false; + } + if (timeout > std::chrono::nanoseconds::zero()) { + timeout -= std::chrono::duration_cast( + std::chrono::steady_clock::now() - stamp); + if (timeout < std::chrono::nanoseconds::zero()) { + timeout = std::chrono::nanoseconds::zero(); + } + } + } + return true; +} + +SyncParametersClient::SyncParametersClient( + rclcpp::Node::SharedPtr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) +: SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + qos_profile) +{} + +SyncParametersClient::SyncParametersClient( + rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Node::SharedPtr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) +: SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile) +{} + +SyncParametersClient::SyncParametersClient( + rclcpp::Node * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) +: SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + qos_profile) +{} + +SyncParametersClient::SyncParametersClient( + rclcpp::executor::Executor::SharedPtr executor, + rclcpp::Node * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) +: SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile) +{} + +SyncParametersClient::SyncParametersClient( + rclcpp::executor::Executor::SharedPtr executor, + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) +: executor_(executor), node_base_interface_(node_base_interface) +{ + async_parameters_client_ = + std::make_shared( + node_base_interface, + node_topics_interface, + node_graph_interface, + node_services_interface, + remote_node_name, + qos_profile); +} + +std::vector +SyncParametersClient::get_parameters(const std::vector & parameter_names) +{ + auto f = async_parameters_client_->get_parameters(parameter_names); + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) + { + return f.get(); + } + // Return an empty vector if unsuccessful + return std::vector(); +} + +bool +SyncParametersClient::has_parameter(const std::string & parameter_name) +{ + std::vector names; + names.push_back(parameter_name); + auto vars = list_parameters(names, 1); + return vars.names.size() > 0; +} + +std::vector +SyncParametersClient::get_parameter_types(const std::vector & parameter_names) +{ + auto f = async_parameters_client_->get_parameter_types(parameter_names); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + +std::vector +SyncParametersClient::set_parameters( + const std::vector & parameters) +{ + auto f = async_parameters_client_->set_parameters(parameters); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + +rcl_interfaces::msg::SetParametersResult +SyncParametersClient::set_parameters_atomically( + const std::vector & parameters) +{ + auto f = async_parameters_client_->set_parameters_atomically(parameters); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) + { + return f.get(); + } + + throw std::runtime_error("Unable to get result of set parameters service call."); +} + +rcl_interfaces::msg::ListParametersResult +SyncParametersClient::list_parameters( + const std::vector & parameter_prefixes, + uint64_t depth) +{ + auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, + f) == rclcpp::executor::FutureReturnCode::SUCCESS) + { + return f.get(); + } + + throw std::runtime_error("Unable to get result of list parameters service call."); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_events_filter.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_events_filter.cpp new file mode 100644 index 0000000..36ef708 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_events_filter.cpp @@ -0,0 +1,60 @@ +// Copyright 2017 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 "rclcpp/parameter_events_filter.hpp" + +#include +#include + +using rclcpp::ParameterEventsFilter; +using EventType = rclcpp::ParameterEventsFilter::EventType; +using EventPair = rclcpp::ParameterEventsFilter::EventPair; + +ParameterEventsFilter::ParameterEventsFilter( + rcl_interfaces::msg::ParameterEvent::SharedPtr event, + const std::vector & names, + const std::vector & types) +: event_(event) +{ + if (std::find(types.begin(), types.end(), EventType::NEW) != types.end()) { + for (auto & new_parameter : event->new_parameters) { + if (std::find(names.begin(), names.end(), new_parameter.name) != names.end()) { + result_.push_back( + EventPair(EventType::NEW, &new_parameter)); + } + } + } + if (std::find(types.begin(), types.end(), EventType::CHANGED) != types.end()) { + for (auto & changed_parameter : event->changed_parameters) { + if (std::find(names.begin(), names.end(), changed_parameter.name) != names.end()) { + result_.push_back( + EventPair(EventType::CHANGED, &changed_parameter)); + } + } + } + if (std::find(types.begin(), types.end(), EventType::DELETED) != types.end()) { + for (auto & deleted_parameter : event->deleted_parameters) { + if (std::find(names.begin(), names.end(), deleted_parameter.name) != names.end()) { + result_.push_back( + EventPair(EventType::DELETED, &deleted_parameter)); + } + } + } +} + +const std::vector & +ParameterEventsFilter::get_events() const +{ + return result_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_map.cpp new file mode 100644 index 0000000..447e50d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_map.cpp @@ -0,0 +1,128 @@ +// Copyright 2018 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 + +#include "rclcpp/parameter_map.hpp" + +using rclcpp::exceptions::InvalidParametersException; +using rclcpp::exceptions::InvalidParameterValueException; +using rclcpp::ParameterMap; +using rclcpp::ParameterValue; + +ParameterMap +rclcpp::parameter_map_from(const rcl_params_t * const c_params) +{ + if (NULL == c_params) { + throw InvalidParametersException("parameters struct is NULL"); + } else if (NULL == c_params->node_names) { + throw InvalidParametersException("node names array is NULL"); + } else if (NULL == c_params->params) { + throw InvalidParametersException("node params array is NULL"); + } + + // Convert c structs into a list of parameters to set + ParameterMap parameters; + for (size_t n = 0; n < c_params->num_nodes; ++n) { + const char * c_node_name = c_params->node_names[n]; + if (NULL == c_node_name) { + throw InvalidParametersException("Node name at index " + std::to_string(n) + " is NULL"); + } + + /// make sure there is a leading slash on the fully qualified node name + std::string node_name("/"); + if ('/' != c_node_name[0]) { + node_name += c_node_name; + } else { + node_name = c_node_name; + } + + const rcl_node_params_t * const c_params_node = &(c_params->params[n]); + + std::vector & params_node = parameters[node_name]; + params_node.reserve(c_params_node->num_params); + + for (size_t p = 0; p < c_params_node->num_params; ++p) { + const char * const c_param_name = c_params_node->parameter_names[p]; + if (NULL == c_param_name) { + std::string message( + "At node " + std::to_string(n) + " parameter " + std::to_string(p) + " name is NULL"); + throw InvalidParametersException(message); + } + const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]); + params_node.emplace_back(c_param_name, parameter_value_from(c_param_value)); + } + } + return parameters; +} + +ParameterValue +rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) +{ + if (NULL == c_param_value) { + throw InvalidParameterValueException("Passed argument is NULL"); + } + if (c_param_value->bool_value) { + return ParameterValue(*(c_param_value->bool_value)); + } else if (c_param_value->integer_value) { + return ParameterValue(*(c_param_value->integer_value)); + } else if (c_param_value->double_value) { + return ParameterValue(*(c_param_value->double_value)); + } else if (c_param_value->string_value) { + return ParameterValue(std::string(c_param_value->string_value)); + } else if (c_param_value->byte_array_value) { + const rcl_byte_array_t * const byte_array = c_param_value->byte_array_value; + std::vector bytes; + bytes.reserve(byte_array->size); + for (size_t v = 0; v < byte_array->size; ++v) { + bytes.push_back(byte_array->values[v]); + } + return ParameterValue(bytes); + } else if (c_param_value->bool_array_value) { + const rcl_bool_array_t * const bool_array = c_param_value->bool_array_value; + std::vector bools; + bools.reserve(bool_array->size); + for (size_t v = 0; v < bool_array->size; ++v) { + bools.push_back(bool_array->values[v]); + } + return ParameterValue(bools); + } else if (c_param_value->integer_array_value) { + const rcl_int64_array_t * const int_array = c_param_value->integer_array_value; + std::vector integers; + integers.reserve(int_array->size); + for (size_t v = 0; v < int_array->size; ++v) { + integers.push_back(int_array->values[v]); + } + return ParameterValue(integers); + } else if (c_param_value->double_array_value) { + const rcl_double_array_t * const double_array = c_param_value->double_array_value; + std::vector doubles; + doubles.reserve(double_array->size); + for (size_t v = 0; v < double_array->size; ++v) { + doubles.push_back(double_array->values[v]); + } + return ParameterValue(doubles); + } else if (c_param_value->string_array_value) { + const rcutils_string_array_t * const string_array = c_param_value->string_array_value; + std::vector strings; + strings.reserve(string_array->size); + for (size_t v = 0; v < string_array->size; ++v) { + strings.emplace_back(string_array->data[v]); + } + return ParameterValue(strings); + } + + throw InvalidParameterValueException("No parameter value set"); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service.cpp new file mode 100644 index 0000000..f1f9166 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service.cpp @@ -0,0 +1,154 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/parameter_service.hpp" + +#include +#include +#include +#include + +#include "./parameter_service_names.hpp" + +using rclcpp::ParameterService; + +ParameterService::ParameterService( + const std::shared_ptr node_base, + const std::shared_ptr node_services, + rclcpp::node_interfaces::NodeParametersInterface * node_params, + const rmw_qos_profile_t & qos_profile) +{ + const std::string node_name = node_base->get_name(); + + get_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::get_parameters, + [node_params]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + for (const auto & name : request->names) { + // Default construct param to NOT_SET + rclcpp::Parameter param; + node_params->get_parameter(name, param); + // push back NOT_SET when get_parameter() call fails + response->values.push_back(param.get_value_message()); + } + }, + qos_profile, nullptr); + + get_parameter_types_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::get_parameter_types, + [node_params]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + try { + auto types = node_params->get_parameter_types(request->names); + std::transform( + types.cbegin(), types.cend(), + std::back_inserter(response->types), [](const uint8_t & type) { + return static_cast(type); + }); + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); + } + }, + qos_profile, nullptr); + + set_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::set_parameters, + [node_params]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + // Set parameters one-by-one, since there's no way to return a partial result if + // set_parameters() fails. + auto result = rcl_interfaces::msg::SetParametersResult(); + for (auto & p : request->parameters) { + try { + result = node_params->set_parameters_atomically( + {rclcpp::Parameter::from_parameter_msg(p)}); + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what()); + result.successful = false; + result.reason = ex.what(); + } + response->results.push_back(result); + } + }, + qos_profile, nullptr); + + set_parameters_atomically_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::set_parameters_atomically, + [node_params]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + std::vector pvariants; + std::transform( + request->parameters.cbegin(), request->parameters.cend(), + std::back_inserter(pvariants), + [](const rcl_interfaces::msg::Parameter & p) { + return rclcpp::Parameter::from_parameter_msg(p); + }); + try { + auto result = node_params->set_parameters_atomically(pvariants); + response->result = result; + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what()); + response->result.successful = false; + response->result.reason = "One or more parameters wer not declared before setting"; + } + }, + qos_profile, nullptr); + + describe_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::describe_parameters, + [node_params]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + try { + auto descriptors = node_params->describe_parameters(request->names); + response->descriptors = descriptors; + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what()); + } + }, + qos_profile, nullptr); + + list_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::list_parameters, + [node_params]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + auto result = node_params->list_parameters(request->prefixes, request->depth); + response->result = result; + }, + qos_profile, nullptr); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service_names.hpp b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service_names.hpp new file mode 100644 index 0000000..9fab588 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_service_names.hpp @@ -0,0 +1,33 @@ +// Copyright 2017 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 RCLCPP__PARAMETER_SERVICE_NAMES_HPP_ +#define RCLCPP__PARAMETER_SERVICE_NAMES_HPP_ + +namespace rclcpp +{ +namespace parameter_service_names +{ + +static constexpr const char * get_parameters = "get_parameters"; +static constexpr const char * get_parameter_types = "get_parameter_types"; +static constexpr const char * set_parameters = "set_parameters"; +static constexpr const char * set_parameters_atomically = "set_parameters_atomically"; +static constexpr const char * describe_parameters = "describe_parameters"; +static constexpr const char * list_parameters = "list_parameters"; + +} // namespace parameter_service_names +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_SERVICE_NAMES_HPP_ diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_value.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_value.cpp new file mode 100644 index 0000000..79c1e13 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/parameter_value.cpp @@ -0,0 +1,241 @@ +// Copyright 2018 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 "rclcpp/parameter_value.hpp" + +#include +#include + +using rclcpp::ParameterType; +using rclcpp::ParameterValue; + +std::string +rclcpp::to_string(const ParameterType type) +{ + switch (type) { + case ParameterType::PARAMETER_NOT_SET: + return "not set"; + case ParameterType::PARAMETER_BOOL: + return "bool"; + case ParameterType::PARAMETER_INTEGER: + return "integer"; + case ParameterType::PARAMETER_DOUBLE: + return "double"; + case ParameterType::PARAMETER_STRING: + return "string"; + case ParameterType::PARAMETER_BYTE_ARRAY: + return "byte_array"; + case ParameterType::PARAMETER_BOOL_ARRAY: + return "bool_array"; + case ParameterType::PARAMETER_INTEGER_ARRAY: + return "integer_array"; + case ParameterType::PARAMETER_DOUBLE_ARRAY: + return "double_array"; + case ParameterType::PARAMETER_STRING_ARRAY: + return "string_array"; + default: + return "unknown type"; + } +} + +std::ostream & +rclcpp::operator<<(std::ostream & os, const ParameterType type) +{ + os << rclcpp::to_string(type); + return os; +} + +template +std::string +array_to_string( + const std::vector & array, + const std::ios::fmtflags format_flags = std::ios::dec) +{ + std::stringstream type_array; + bool first_item = true; + type_array << "["; + type_array.setf(format_flags, std::ios_base::basefield | std::ios::boolalpha); + type_array << std::showbase; + for (const ValType value : array) { + if (!first_item) { + type_array << ", "; + } else { + first_item = false; + } + type_array << static_cast(value); + } + type_array << "]"; + return type_array.str(); +} + +std::string +rclcpp::to_string(const ParameterValue & value) +{ + switch (value.get_type()) { + case ParameterType::PARAMETER_NOT_SET: + return "not set"; + case ParameterType::PARAMETER_BOOL: + return value.get() ? "true" : "false"; + case ParameterType::PARAMETER_INTEGER: + return std::to_string(value.get()); + case ParameterType::PARAMETER_DOUBLE: + return std::to_string(value.get()); + case ParameterType::PARAMETER_STRING: + return value.get(); + case ParameterType::PARAMETER_BYTE_ARRAY: + return array_to_string(value.get>(), std::ios::hex); + case ParameterType::PARAMETER_BOOL_ARRAY: + return array_to_string(value.get>(), std::ios::boolalpha); + case ParameterType::PARAMETER_INTEGER_ARRAY: + return array_to_string(value.get>()); + case ParameterType::PARAMETER_DOUBLE_ARRAY: + return array_to_string(value.get>()); + case ParameterType::PARAMETER_STRING_ARRAY: + return array_to_string(value.get>()); + default: + return "unknown type"; + } +} + +ParameterValue::ParameterValue() +{ + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; +} + +ParameterValue::ParameterValue(const rcl_interfaces::msg::ParameterValue & value) +{ + value_ = value; + switch (value.type) { + case PARAMETER_BOOL: + case PARAMETER_INTEGER: + case PARAMETER_DOUBLE: + case PARAMETER_STRING: + case PARAMETER_BYTE_ARRAY: + case PARAMETER_BOOL_ARRAY: + case PARAMETER_INTEGER_ARRAY: + case PARAMETER_DOUBLE_ARRAY: + case PARAMETER_STRING_ARRAY: + case PARAMETER_NOT_SET: + break; + default: + // TODO(wjwwood): use custom exception + throw std::runtime_error("Unknown type: " + std::to_string(value.type)); + } +} + +ParameterValue::ParameterValue(const bool bool_value) +{ + value_.bool_value = bool_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; +} + +ParameterValue::ParameterValue(const int int_value) +{ + value_.integer_value = int_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; +} + +ParameterValue::ParameterValue(const int64_t int_value) +{ + value_.integer_value = int_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; +} + +ParameterValue::ParameterValue(const float double_value) +{ + value_.double_value = double_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; +} + +ParameterValue::ParameterValue(const double double_value) +{ + value_.double_value = double_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; +} + +ParameterValue::ParameterValue(const std::string & string_value) +{ + value_.string_value = string_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; +} + +ParameterValue::ParameterValue(const char * string_value) +: ParameterValue(std::string(string_value)) +{} + +ParameterValue::ParameterValue(const std::vector & byte_array_value) +{ + value_.byte_array_value = byte_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & bool_array_value) +{ + value_.bool_array_value = bool_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & int_array_value) +{ + value_.integer_array_value.assign(int_array_value.cbegin(), int_array_value.cend()); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & int_array_value) +{ + value_.integer_array_value = int_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & float_array_value) +{ + value_.double_array_value.assign(float_array_value.cbegin(), float_array_value.cend()); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & double_array_value) +{ + value_.double_array_value = double_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & string_array_value) +{ + value_.string_array_value = string_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; +} + +ParameterType +ParameterValue::get_type() const +{ + return static_cast(value_.type); +} + +rcl_interfaces::msg::ParameterValue +ParameterValue::to_value_msg() const +{ + return value_; +} + +bool +ParameterValue::operator==(const ParameterValue & rhs) const +{ + return this->value_ == rhs.value_; +} + +bool +ParameterValue::operator!=(const ParameterValue & rhs) const +{ + return this->value_ != rhs.value_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/publisher_base.cpp new file mode 100644 index 0000000..cb96a14 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/publisher_base.cpp @@ -0,0 +1,246 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/publisher_base.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rcutils/logging_macros.h" +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" + +using rclcpp::PublisherBase; + +PublisherBase::PublisherBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rosidl_message_type_support_t & type_support, + const rcl_publisher_options_t & publisher_options) +: rcl_node_handle_(node_base->get_shared_rcl_node_handle()), + intra_process_is_enabled_(false), intra_process_publisher_id_(0) +{ + rcl_ret_t ret = rcl_publisher_init( + &publisher_handle_, + rcl_node_handle_.get(), + &type_support, + topic.c_str(), + &publisher_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = rcl_node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + topic, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); + } + // Life time of this object is tied to the publisher handle. + rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); + if (!publisher_rmw_handle) { + auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { + auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string().str; + rmw_reset_error(); + throw std::runtime_error(msg); + } +} + +PublisherBase::~PublisherBase() +{ + // must fini the events before fini-ing the publisher + event_handlers_.clear(); + + if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl publisher handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + + auto ipm = weak_ipm_.lock(); + + if (!intra_process_is_enabled_) { + return; + } + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than a publisher."); + return; + } + ipm->remove_publisher(intra_process_publisher_id_); +} + +const char * +PublisherBase::get_topic_name() const +{ + return rcl_publisher_get_topic_name(&publisher_handle_); +} + +size_t +PublisherBase::get_queue_size() const +{ + const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_); + if (!publisher_options) { + auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + return publisher_options->qos.depth; +} + +const rmw_gid_t & +PublisherBase::get_gid() const +{ + return rmw_gid_; +} + +rcl_publisher_t * +PublisherBase::get_publisher_handle() +{ + return &publisher_handle_; +} + +const rcl_publisher_t * +PublisherBase::get_publisher_handle() const +{ + return &publisher_handle_; +} + +const std::vector> & +PublisherBase::get_event_handlers() const +{ + return event_handlers_; +} + +size_t +PublisherBase::get_subscription_count() const +{ + size_t inter_process_subscription_count = 0; + + rcl_ret_t status = rcl_publisher_get_subscription_count( + &publisher_handle_, + &inter_process_subscription_count); + + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); /* next call will reset error message if not context */ + if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { + rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); + if (nullptr != context && !rcl_context_is_valid(context)) { + /* publisher is invalid due to context being shutdown */ + return 0; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get subscription count"); + } + return inter_process_subscription_count; +} + +size_t +PublisherBase::get_intra_process_subscription_count() const +{ + auto ipm = weak_ipm_.lock(); + if (!intra_process_is_enabled_) { + return 0; + } + if (!ipm) { + // TODO(ivanpauno): should this just return silently? Or maybe return with a warning? + // Same as wjwwood comment in publisher_factory create_shared_publish_callback. + throw std::runtime_error( + "intra process subscriber count called after " + "destruction of intra process manager"); + } + return ipm->get_subscription_count(intra_process_publisher_id_); +} + +rclcpp::QoS +PublisherBase::get_actual_qos() const +{ + const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_); + if (!qos) { + auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); +} + +bool +PublisherBase::assert_liveliness() const +{ + return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_); +} + +bool +PublisherBase::can_loan_messages() const +{ + return rcl_publisher_can_loan_messages(&publisher_handle_); +} + +bool +PublisherBase::operator==(const rmw_gid_t & gid) const +{ + return *this == &gid; +} + +bool +PublisherBase::operator==(const rmw_gid_t * gid) const +{ + bool result = false; + auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); + if (ret != RMW_RET_OK) { + auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str; + rmw_reset_error(); + throw std::runtime_error(msg); + } + return result; +} + +void +PublisherBase::setup_intra_process( + uint64_t intra_process_publisher_id, + IntraProcessManagerSharedPtr ipm) +{ + intra_process_publisher_id_ = intra_process_publisher_id; + weak_ipm_ = ipm; + intra_process_is_enabled_ = true; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/qos.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/qos.cpp new file mode 100644 index 0000000..bec92fd --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/qos.cpp @@ -0,0 +1,207 @@ +// Copyright 2019 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 "rclcpp/qos.hpp" + +#include + +namespace rclcpp +{ + +QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg) +: history_policy(history_policy_arg), depth(depth_arg) +{} + +QoSInitialization +QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) +{ + switch (rmw_qos.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + return KeepAll(); + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + default: + return KeepLast(rmw_qos.depth); + } +} + +KeepAll::KeepAll() +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) +{} + +KeepLast::KeepLast(size_t depth) +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) +{} + +QoS::QoS( + const QoSInitialization & qos_initialization, + const rmw_qos_profile_t & initial_profile) +: rmw_qos_profile_(initial_profile) +{ + rmw_qos_profile_.history = qos_initialization.history_policy; + rmw_qos_profile_.depth = qos_initialization.depth; +} + +QoS::QoS(size_t history_depth) +: QoS(KeepLast(history_depth)) +{} + +rmw_qos_profile_t & +QoS::get_rmw_qos_profile() +{ + return rmw_qos_profile_; +} + +const rmw_qos_profile_t & +QoS::get_rmw_qos_profile() const +{ + return rmw_qos_profile_; +} + +QoS & +QoS::history(rmw_qos_history_policy_t history) +{ + rmw_qos_profile_.history = history; + return *this; +} + +QoS & +QoS::keep_last(size_t depth) +{ + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + rmw_qos_profile_.depth = depth; + return *this; +} + +QoS & +QoS::keep_all() +{ + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + rmw_qos_profile_.depth = 0; + return *this; +} + +QoS & +QoS::reliability(rmw_qos_reliability_policy_t reliability) +{ + rmw_qos_profile_.reliability = reliability; + return *this; +} + +QoS & +QoS::reliable() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); +} + +QoS & +QoS::best_effort() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); +} + +QoS & +QoS::durability(rmw_qos_durability_policy_t durability) +{ + rmw_qos_profile_.durability = durability; + return *this; +} + +QoS & +QoS::durability_volatile() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); +} + +QoS & +QoS::transient_local() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); +} + +QoS & +QoS::deadline(rmw_time_t deadline) +{ + rmw_qos_profile_.deadline = deadline; + return *this; +} + +QoS & +QoS::deadline(const rclcpp::Duration & deadline) +{ + return this->deadline(deadline.to_rmw_time()); +} + +QoS & +QoS::lifespan(rmw_time_t lifespan) +{ + rmw_qos_profile_.lifespan = lifespan; + return *this; +} + +QoS & +QoS::lifespan(const rclcpp::Duration & lifespan) +{ + return this->lifespan(lifespan.to_rmw_time()); +} + +QoS & +QoS::liveliness(rmw_qos_liveliness_policy_t liveliness) +{ + rmw_qos_profile_.liveliness = liveliness; + return *this; +} + +QoS & +QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration) +{ + rmw_qos_profile_.liveliness_lease_duration = liveliness_lease_duration; + return *this; +} + +QoS & +QoS::liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration) +{ + return this->liveliness_lease_duration(liveliness_lease_duration.to_rmw_time()); +} + +QoS & +QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions) +{ + rmw_qos_profile_.avoid_ros_namespace_conventions = avoid_ros_namespace_conventions; + return *this; +} + +SensorDataQoS::SensorDataQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_sensor_data) +{} + +ParametersQoS::ParametersQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_parameters) +{} + +ServicesQoS::ServicesQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_services_default) +{} + +ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_parameter_events) +{} + +SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_system_default) +{} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/qos_event.cpp new file mode 100644 index 0000000..75072f7 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/qos_event.cpp @@ -0,0 +1,55 @@ +// Copyright 2019 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 "rclcpp/qos_event.hpp" + +namespace rclcpp +{ + +QOSEventHandlerBase::~QOSEventHandlerBase() +{ + if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } +} + +/// Get the number of ready events. +size_t +QOSEventHandlerBase::get_number_of_ready_events() +{ + return 1; +} + +/// Add the Waitable to a wait set. +bool +QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); + } + return true; +} + +/// Check if the Waitable is ready. +bool +QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +{ + return wait_set->events[wait_set_event_index_] == &event_handle_; +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/service.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/service.cpp new file mode 100644 index 0000000..c7f8ff9 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/service.cpp @@ -0,0 +1,65 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/service.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/macros.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +using rclcpp::ServiceBase; + +ServiceBase::ServiceBase(std::shared_ptr node_handle) +: node_handle_(node_handle) +{} + +ServiceBase::~ServiceBase() +{} + +const char * +ServiceBase::get_service_name() +{ + return rcl_service_get_service_name(this->get_service_handle().get()); +} + +std::shared_ptr +ServiceBase::get_service_handle() +{ + return service_handle_; +} + +std::shared_ptr +ServiceBase::get_service_handle() const +{ + return service_handle_; +} + +rcl_node_t * +ServiceBase::get_rcl_node_handle() +{ + return node_handle_.get(); +} + +const rcl_node_t * +ServiceBase::get_rcl_node_handle() const +{ + return node_handle_.get(); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.cpp new file mode 100644 index 0000000..dbdc1ae --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.cpp @@ -0,0 +1,379 @@ +// Copyright 2018 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 "./signal_handler.hpp" + +#include +#include +#include +#include +#include + +// includes for semaphore notification code +#if defined(_WIN32) +#include +#elif defined(__APPLE__) +#include +#else // posix +#include +#endif + +#include "rclcpp/logging.hpp" +#include "rmw/impl/cpp/demangle.hpp" + +using rclcpp::SignalHandler; + +// initialize static storage in SignalHandler class +SignalHandler::signal_handler_type SignalHandler::old_signal_handler_; +std::atomic_bool SignalHandler::signal_received_ = ATOMIC_VAR_INIT(false); +std::atomic_bool SignalHandler::wait_for_signal_is_setup_ = ATOMIC_VAR_INIT(false); +#if defined(_WIN32) +HANDLE SignalHandler::signal_handler_sem_; +#elif defined(__APPLE__) +dispatch_semaphore_t SignalHandler::signal_handler_sem_; +#else // posix +sem_t SignalHandler::signal_handler_sem_; +#endif + +// The logger must be initialized before the local static variable signal_handler, +// from the method get_global_signal_handler(), so that it is destructed after +// it, because the destructor of SignalHandler uses this logger object. +static rclcpp::Logger g_logger = rclcpp::get_logger("rclcpp"); + +rclcpp::Logger & +SignalHandler::get_logger() +{ + return g_logger; +} + +SignalHandler & +SignalHandler::get_global_signal_handler() +{ + // This is initialized after the g_logger static global, ensuring + // SignalHandler::get_logger() may be called from the destructor of + // SignalHandler, according to this: + // + // Variables declared at block scope with the specifier static have static + // storage duration but are initialized the first time control passes + // through their declaration (unless their initialization is zero- or + // constant-initialization, which can be performed before the block is + // first entered). On all further calls, the declaration is skipped. + // + // -- https://en.cppreference.com/w/cpp/language/storage_duration#Static_local_variables + // + // Which is guaranteed to occur after static initialization for global (see: + // https://en.cppreference.com/w/cpp/language/initialization#Static_initialization), + // which is when g_logger will be initialized. + // And destruction will occur in the reverse order. + static SignalHandler signal_handler; + return signal_handler; +} + +bool +SignalHandler::install() +{ + std::lock_guard lock(install_mutex_); + bool already_installed = installed_.exchange(true); + if (already_installed) { + return false; + } + try { + setup_wait_for_signal(); + signal_received_.store(false); + + SignalHandler::signal_handler_type signal_handler_argument; +#if defined(RCLCPP_HAS_SIGACTION) + memset(&signal_handler_argument, 0, sizeof(signal_handler_argument)); + sigemptyset(&signal_handler_argument.sa_mask); + signal_handler_argument.sa_sigaction = signal_handler; + signal_handler_argument.sa_flags = SA_SIGINFO; +#else + signal_handler_argument = signal_handler; +#endif + + old_signal_handler_ = SignalHandler::set_signal_handler(SIGINT, signal_handler_argument); + + signal_handler_thread_ = std::thread(&SignalHandler::deferred_signal_handler, this); + } catch (...) { + installed_.store(false); + throw; + } + RCLCPP_DEBUG(get_logger(), "signal handler installed"); + return true; +} + +bool +SignalHandler::uninstall() +{ + std::lock_guard lock(install_mutex_); + bool installed = installed_.exchange(false); + if (!installed) { + return false; + } + try { + // TODO(wjwwood): what happens if someone overrides our signal handler then calls uninstall? + // I think we need to assert that we're the current signal handler, and mitigate if not. + set_signal_handler(SIGINT, old_signal_handler_); + RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler"); + notify_signal_handler(); + signal_handler_thread_.join(); + teardown_wait_for_signal(); + } catch (...) { + installed_.exchange(true); + throw; + } + RCLCPP_DEBUG(get_logger(), "signal handler uninstalled"); + return true; +} + +bool +SignalHandler::is_installed() +{ + return installed_.load(); +} + +SignalHandler::~SignalHandler() +{ + try { + uninstall(); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + get_logger(), + "caught %s exception when uninstalling the sigint handler in rclcpp::~SignalHandler: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + get_logger(), + "caught unknown exception when uninstalling the sigint handler in rclcpp::~SignalHandler"); + } +} + +RCLCPP_LOCAL +void +__safe_strerror(int errnum, char * buffer, size_t buffer_length) +{ +#if defined(_WIN32) + strerror_s(buffer, buffer_length, errnum); +#elif defined(_GNU_SOURCE) && (!defined(ANDROID) || __ANDROID_API__ >= 23) + /* GNU-specific */ + char * msg = strerror_r(errnum, buffer, buffer_length); + if (msg != buffer) { + strncpy(buffer, msg, buffer_length); + buffer[buffer_length - 1] = '\0'; + } +#else + /* XSI-compliant */ + int error_status = strerror_r(errnum, buffer, buffer_length); + if (error_status != 0) { + throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errnum)); + } +#endif +} + +SignalHandler::signal_handler_type +SignalHandler::set_signal_handler( + int signal_value, + const SignalHandler::signal_handler_type & signal_handler) +{ + bool signal_handler_install_failed; + SignalHandler::signal_handler_type old_signal_handler; +#if defined(RCLCPP_HAS_SIGACTION) + ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler); + signal_handler_install_failed = (ret == -1); +#else + old_signal_handler = std::signal(signal_value, signal_handler); + signal_handler_install_failed = (old_signal_handler == SIG_ERR); +#endif + if (signal_handler_install_failed) { + char error_string[1024]; + __safe_strerror(errno, error_string, sizeof(error_string)); + auto msg = + "Failed to set SIGINT signal handler (" + std::to_string(errno) + "): " + error_string; + throw std::runtime_error(msg); + } + + return old_signal_handler; +} + +void +SignalHandler::signal_handler_common() +{ + signal_received_.store(true); + RCLCPP_DEBUG( + get_logger(), + "signal_handler(): SIGINT received, notifying deferred signal handler"); + notify_signal_handler(); +} + +#if defined(RCLCPP_HAS_SIGACTION) +void +SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * context) +{ + RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); + + if (old_signal_handler_.sa_flags & SA_SIGINFO) { + if (old_signal_handler_.sa_sigaction != NULL) { + old_signal_handler_.sa_sigaction(signal_value, siginfo, context); + } + } else { + if ( + old_signal_handler_.sa_handler != NULL && // Is set + old_signal_handler_.sa_handler != SIG_DFL && // Is not default + old_signal_handler_.sa_handler != SIG_IGN) // Is not ignored + { + old_signal_handler_.sa_handler(signal_value); + } + } + + signal_handler_common(); +} +#else +void +SignalHandler::signal_handler(int signal_value) +{ + RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); + + if (old_signal_handler_) { + old_signal_handler_(signal_value); + } + + signal_handler_common(); +} +#endif + +void +SignalHandler::deferred_signal_handler() +{ + while (true) { + if (signal_received_.exchange(false)) { + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): SIGINT received, shutting down"); + for (auto context_ptr : rclcpp::get_contexts()) { + if (context_ptr->get_init_options().shutdown_on_sigint) { + RCLCPP_DEBUG( + get_logger(), + "deferred_signal_handler(): " + "shutting down rclcpp::Context @ %p, because it had shutdown_on_sigint == true", + context_ptr.get()); + context_ptr->shutdown("signal handler"); + } + } + } + if (!is_installed()) { + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): signal handling uninstalled"); + break; + } + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): waiting for SIGINT or uninstall"); + wait_for_signal(); + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): woken up due to SIGINT or uninstall"); + } +} + +void +SignalHandler::setup_wait_for_signal() +{ +#if defined(_WIN32) + signal_handler_sem_ = CreateSemaphore( + NULL, // default security attributes + 0, // initial semaphore count + 1, // maximum semaphore count + NULL); // unnamed semaphore + if (NULL == signal_handler_sem_) { + throw std::runtime_error("CreateSemaphore() failed in setup_wait_for_signal()"); + } +#elif defined(__APPLE__) + signal_handler_sem_ = dispatch_semaphore_create(0); +#else // posix + if (-1 == sem_init(&signal_handler_sem_, 0, 0)) { + throw std::runtime_error(std::string("sem_init() failed: ") + strerror(errno)); + } +#endif + wait_for_signal_is_setup_.store(true); +} + +void +SignalHandler::teardown_wait_for_signal() noexcept +{ + if (!wait_for_signal_is_setup_.exchange(false)) { + return; + } +#if defined(_WIN32) + CloseHandle(signal_handler_sem_); +#elif defined(__APPLE__) + dispatch_release(signal_handler_sem_); +#else // posix + if (-1 == sem_destroy(&signal_handler_sem_)) { + RCLCPP_ERROR(get_logger(), "invalid semaphore in teardown_wait_for_signal()"); + } +#endif +} + +void +SignalHandler::wait_for_signal() +{ + if (!wait_for_signal_is_setup_.load()) { + RCLCPP_ERROR(get_logger(), "called wait_for_signal() before setup_wait_for_signal()"); + return; + } +#if defined(_WIN32) + DWORD dw_wait_result = WaitForSingleObject(signal_handler_sem_, INFINITE); + switch (dw_wait_result) { + case WAIT_ABANDONED: + RCLCPP_ERROR( + get_logger(), "WaitForSingleObject() failed in wait_for_signal() with WAIT_ABANDONED: %s", + GetLastError()); + break; + case WAIT_OBJECT_0: + // successful + break; + case WAIT_TIMEOUT: + RCLCPP_ERROR(get_logger(), "WaitForSingleObject() timedout out in wait_for_signal()"); + break; + case WAIT_FAILED: + RCLCPP_ERROR( + get_logger(), "WaitForSingleObject() failed in wait_for_signal(): %s", GetLastError()); + break; + default: + RCLCPP_ERROR( + get_logger(), "WaitForSingleObject() gave unknown return in wait_for_signal(): %s", + GetLastError()); + } +#elif defined(__APPLE__) + dispatch_semaphore_wait(signal_handler_sem_, DISPATCH_TIME_FOREVER); +#else // posix + int s; + do { + s = sem_wait(&signal_handler_sem_); + } while (-1 == s && EINTR == errno); +#endif +} + +void +SignalHandler::notify_signal_handler() noexcept +{ + if (!wait_for_signal_is_setup_.load()) { + return; + } +#if defined(_WIN32) + if (!ReleaseSemaphore(signal_handler_sem_, 1, NULL)) { + RCLCPP_ERROR( + get_logger(), "ReleaseSemaphore() failed in notify_signal_handler(): %s", GetLastError()); + } +#elif defined(__APPLE__) + dispatch_semaphore_signal(signal_handler_sem_); +#else // posix + if (-1 == sem_post(&signal_handler_sem_)) { + RCLCPP_ERROR(get_logger(), "sem_post failed in notify_signal_handler()"); + } +#endif +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.hpp new file mode 100644 index 0000000..81cb6e1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/signal_handler.hpp @@ -0,0 +1,189 @@ +// Copyright 2018 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 RCLCPP__SIGNAL_HANDLER_HPP_ +#define RCLCPP__SIGNAL_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/logging.hpp" + +// includes for semaphore notification code +#if defined(_WIN32) +#include +#elif defined(__APPLE__) +#include +#else // posix +#include +#endif + +// Determine if sigaction is available +#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE +#define RCLCPP_HAS_SIGACTION +#endif + +namespace rclcpp +{ + +/// Responsible for manaaging the SIGINT signal handling. +/** + * This class is responsible for: + * + * - installing the signal handler for SIGINT + * - uninstalling the signal handler for SIGINT + * - creating a thread to execute "on sigint" work outside of the signal handler + * - safely notifying the dedicated signal handling thread when receiving SIGINT + * - implementation of all of the signal handling work, like shutting down contexts + * + * \internal + */ +class SignalHandler final +{ +public: + /// Return the global singleton of this class. + static + SignalHandler & + get_global_signal_handler(); + + /// Return a global singleton logger to avoid needing to create it everywhere. + static + rclcpp::Logger & + get_logger(); + + /// Install the signal handler for SIGINT and start the dedicated signal handling thread. + /** + * Also stores the current signal handler to be called on SIGINT and to + * restore when uninstalling this signal handler. + */ + bool + install(); + + /// Uninstall the signal handler for SIGINT and join the dedicated singal handling thread. + /** + * Also restores the previous signal handler. + */ + bool + uninstall(); + + /// Return true if installed, false otherwise. + bool + is_installed(); + +private: + SignalHandler() = default; + + ~SignalHandler(); + +#if defined(RCLCPP_HAS_SIGACTION) + using signal_handler_type = struct sigaction; +#else + using signal_handler_type = void (*)(int); +#endif + // POSIX signal handler structure storage for the existing signal handler. + static SignalHandler::signal_handler_type old_signal_handler_; + + /// Set the signal handler function. + static + SignalHandler::signal_handler_type + set_signal_handler(int signal_value, const SignalHandler::signal_handler_type & signal_handler); + + /// Common signal handler code between sigaction and non-sigaction versions. + static + void + signal_handler_common(); + +#if defined(RCLCPP_HAS_SIGACTION) + /// Signal handler function. + static + void + signal_handler(int signal_value, siginfo_t * siginfo, void * context); +#else + /// Signal handler function. + static + void + signal_handler(int signal_value); +#endif + + /// Target of the dedicated signal handling thread. + void + deferred_signal_handler(); + + /// Setup anything that is necessary for wait_for_signal() or notify_signal_handler(). + /** + * This must be called before wait_for_signal() or notify_signal_handler(). + * This is not thread-safe. + */ + static + void + setup_wait_for_signal(); + + /// Undo all setup done in setup_wait_for_signal(). + /** + * Must not call wait_for_signal() or notify_signal_handler() after calling this. + * + * This is not thread-safe. + */ + static + void + teardown_wait_for_signal() noexcept; + + /// Wait for a notification from notify_signal_handler() in a signal safe way. + /** + * This static method may throw if posting the semaphore fails. + * + * This is not thread-safe. + */ + static + void + wait_for_signal(); + + /// Notify blocking wait_for_signal() calls in a signal safe way. + /** + * This is used to notify the deferred_signal_handler() thread to start work + * from the signal handler. + * + * This is thread-safe. + */ + static + void + notify_signal_handler() noexcept; + + // Whether or not a signal has been received. + static std::atomic_bool signal_received_; + // A thread to which singal handling tasks are deferred. + std::thread signal_handler_thread_; + + // A mutex used to synchronize the install() and uninstall() methods. + std::mutex install_mutex_; + // Whether or not the signal handler has been installed. + std::atomic_bool installed_{false}; + + // Whether or not the semaphore for wait_for_signal is setup. + static std::atomic_bool wait_for_signal_is_setup_; + // Storage for the wait_for_signal semaphore. +#if defined(_WIN32) + static HANDLE signal_handler_sem_; +#elif defined(__APPLE__) + static dispatch_semaphore_t signal_handler_sem_; +#else // posix + static sem_t signal_handler_sem_; +#endif +}; + +} // namespace rclcpp + +#endif // RCLCPP__SIGNAL_HANDLER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/subscription_base.cpp new file mode 100644 index 0000000..314a8b8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/subscription_base.cpp @@ -0,0 +1,211 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/subscription_base.hpp" + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +using rclcpp::SubscriptionBase; + +SubscriptionBase::SubscriptionBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, + const std::string & topic_name, + const rcl_subscription_options_t & subscription_options, + bool is_serialized) +: node_base_(node_base), + node_handle_(node_base_->get_shared_rcl_node_handle()), + use_intra_process_(false), + intra_process_subscription_id_(0), + type_support_(type_support_handle), + is_serialized_(is_serialized) +{ + auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) + { + if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"), + "Error in destruction of rcl subscription handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete rcl_subs; + }; + + subscription_handle_ = std::shared_ptr( + new rcl_subscription_t, custom_deletor); + *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + + rcl_ret_t ret = rcl_subscription_init( + subscription_handle_.get(), + node_handle_.get(), + &type_support_handle, + topic_name.c_str(), + &subscription_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); + } +} + +SubscriptionBase::~SubscriptionBase() +{ + if (!use_intra_process_) { + return; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than a subscription."); + return; + } + ipm->remove_subscription(intra_process_subscription_id_); +} + +const char * +SubscriptionBase::get_topic_name() const +{ + return rcl_subscription_get_topic_name(subscription_handle_.get()); +} + +std::shared_ptr +SubscriptionBase::get_subscription_handle() +{ + return subscription_handle_; +} + +const std::shared_ptr +SubscriptionBase::get_subscription_handle() const +{ + return subscription_handle_; +} + +const std::vector> & +SubscriptionBase::get_event_handlers() const +{ + return event_handlers_; +} + +rclcpp::QoS +SubscriptionBase::get_actual_qos() const +{ + const rmw_qos_profile_t * qos = rcl_subscription_get_actual_qos(subscription_handle_.get()); + if (!qos) { + auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); +} + +const rosidl_message_type_support_t & +SubscriptionBase::get_message_type_support_handle() const +{ + return type_support_; +} + +bool +SubscriptionBase::is_serialized() const +{ + return is_serialized_; +} + +size_t +SubscriptionBase::get_publisher_count() const +{ + size_t inter_process_publisher_count = 0; + + rmw_ret_t status = rcl_subscription_get_publisher_count( + subscription_handle_.get(), + &inter_process_publisher_count); + + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get publisher count"); + } + return inter_process_publisher_count; +} + +void +SubscriptionBase::setup_intra_process( + uint64_t intra_process_subscription_id, + IntraProcessManagerWeakPtr weak_ipm) +{ + intra_process_subscription_id_ = intra_process_subscription_id; + weak_ipm_ = weak_ipm; + use_intra_process_ = true; +} + +bool +SubscriptionBase::can_loan_messages() const +{ + return rcl_subscription_can_loan_messages(subscription_handle_.get()); +} + +rclcpp::Waitable::SharedPtr +SubscriptionBase::get_intra_process_waitable() const +{ + // If not using intra process, shortcut to nullptr. + if (!use_intra_process_) { + return nullptr; + } + // Get the intra process manager. + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "SubscriptionBase::get_intra_process_waitable() called " + "after destruction of intra process manager"); + } + + // Use the id to retrieve the subscription intra-process from the intra-process manager. + return ipm->get_subscription_intra_process(intra_process_subscription_id_); +} + +bool +SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const +{ + if (!use_intra_process_) { + return false; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publisher check called " + "after destruction of intra process manager"); + } + return ipm->matches_any_publishers(sender_gid); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/subscription_intra_process_base.cpp new file mode 100644 index 0000000..3b951f3 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -0,0 +1,38 @@ +// Copyright 2019 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 "rclcpp/experimental/subscription_intra_process_base.hpp" + +using rclcpp::experimental::SubscriptionIntraProcessBase; + +bool +SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(reentrant_mutex_); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); + return RCL_RET_OK == ret; +} + +const char * +SubscriptionIntraProcessBase::get_topic_name() const +{ + return topic_name_.c_str(); +} + +rmw_qos_profile_t +SubscriptionIntraProcessBase::get_actual_qos() const +{ + return qos_profile_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/time.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/time.cpp new file mode 100644 index 0000000..b7f0bd2 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/time.cpp @@ -0,0 +1,265 @@ +// Copyright 2017 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 +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" + +#include "builtin_interfaces/msg/time.hpp" + +#include "rcl/time.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/logging_macros.h" + +#include "rclcpp/utilities.hpp" + +namespace +{ + +rcl_time_point_t +init_time_point(rcl_clock_type_t & clock_type) +{ + rcl_time_point_t time_point; + time_point.clock_type = clock_type; + + return time_point; +} + +} // namespace + +namespace rclcpp +{ + +Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type) +: rcl_time_(init_time_point(clock_type)) +{ + if (seconds < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } + + rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast(seconds)); + rcl_time_.nanoseconds += nanoseconds; +} + +Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) +: rcl_time_(init_time_point(clock_type)) +{ + rcl_time_.nanoseconds = nanoseconds; +} + +Time::Time(const Time & rhs) +: rcl_time_(rhs.rcl_time_) +{ + rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds; +} + +Time::Time( + const builtin_interfaces::msg::Time & time_msg, + rcl_clock_type_t ros_time) +{ + rcl_time_ = init_time_point(ros_time); + if (time_msg.sec < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } + + rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast(time_msg.sec)); + rcl_time_.nanoseconds += time_msg.nanosec; +} + +Time::Time(const rcl_time_point_t & time_point) +: rcl_time_(time_point) +{ + // noop +} + +Time::~Time() +{ +} + +Time::operator builtin_interfaces::msg::Time() const +{ + builtin_interfaces::msg::Time msg_time; + msg_time.sec = static_cast(RCL_NS_TO_S(rcl_time_.nanoseconds)); + msg_time.nanosec = static_cast(rcl_time_.nanoseconds % (1000 * 1000 * 1000)); + return msg_time; +} + +Time & +Time::operator=(const Time & rhs) +{ + rcl_time_ = rhs.rcl_time_; + return *this; +} + +Time & +Time::operator=(const builtin_interfaces::msg::Time & time_msg) +{ + if (time_msg.sec < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } + + + rcl_clock_type_t ros_time = RCL_ROS_TIME; + rcl_time_ = init_time_point(ros_time); // TODO(tfoote) hard coded ROS here + + rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast(time_msg.sec)); + rcl_time_.nanoseconds += time_msg.nanosec; + return *this; +} + +bool +Time::operator==(const rclcpp::Time & rhs) const +{ + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { + throw std::runtime_error("can't compare times with different time sources"); + } + + return rcl_time_.nanoseconds == rhs.rcl_time_.nanoseconds; +} + +bool +Time::operator!=(const rclcpp::Time & rhs) const +{ + return !(*this == rhs); +} + +bool +Time::operator<(const rclcpp::Time & rhs) const +{ + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { + throw std::runtime_error("can't compare times with different time sources"); + } + + return rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds; +} + +bool +Time::operator<=(const rclcpp::Time & rhs) const +{ + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { + throw std::runtime_error("can't compare times with different time sources"); + } + + return rcl_time_.nanoseconds <= rhs.rcl_time_.nanoseconds; +} + +bool +Time::operator>=(const rclcpp::Time & rhs) const +{ + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { + throw std::runtime_error("can't compare times with different time sources"); + } + + return rcl_time_.nanoseconds >= rhs.rcl_time_.nanoseconds; +} + +bool +Time::operator>(const rclcpp::Time & rhs) const +{ + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { + throw std::runtime_error("can't compare times with different time sources"); + } + + return rcl_time_.nanoseconds > rhs.rcl_time_.nanoseconds; +} + +Time +Time::operator+(const rclcpp::Duration & rhs) const +{ + if (rclcpp::add_will_overflow(rhs.nanoseconds(), this->nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::add_will_underflow(rhs.nanoseconds(), this->nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + return Time(this->nanoseconds() + rhs.nanoseconds(), this->get_clock_type()); +} + +Duration +Time::operator-(const rclcpp::Time & rhs) const +{ + if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { + throw std::runtime_error( + std::string("can't subtract times with different time sources [") + + std::to_string(rcl_time_.clock_type) + " != " + + std::to_string(rhs.rcl_time_.clock_type) + "]"); + } + + if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) { + throw std::overflow_error("time subtraction leads to int64_t overflow"); + } + + if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) { + throw std::underflow_error("time subtraction leads to int64_t underflow"); + } + + return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds); +} + +Time +Time::operator-(const rclcpp::Duration & rhs) const +{ + if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.nanoseconds())) { + throw std::overflow_error("time subtraction leads to int64_t overflow"); + } + if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.nanoseconds())) { + throw std::underflow_error("time subtraction leads to int64_t underflow"); + } + + return Time(rcl_time_.nanoseconds - rhs.nanoseconds(), rcl_time_.clock_type); +} + +int64_t +Time::nanoseconds() const +{ + return rcl_time_.nanoseconds; +} + +double +Time::seconds() const +{ + return std::chrono::duration(std::chrono::nanoseconds(rcl_time_.nanoseconds)).count(); +} + +rcl_clock_type_t +Time::get_clock_type() const +{ + return rcl_time_.clock_type; +} + +Time +operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs) +{ + if (rclcpp::add_will_overflow(rhs.nanoseconds(), lhs.nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::add_will_underflow(rhs.nanoseconds(), lhs.nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type()); +} + +Time +Time::max() +{ + return Time(std::numeric_limits::max(), 999999999); +} + + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/time_source.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/time_source.cpp new file mode 100644 index 0000000..ccf05bf --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/time_source.cpp @@ -0,0 +1,333 @@ +// Copyright 2017 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 +#include +#include + +#include "builtin_interfaces/msg/time.hpp" + +#include "rcl/time.h" + +#include "rclcpp/clock.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/parameter_events_filter.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" + +namespace rclcpp +{ + +TimeSource::TimeSource(std::shared_ptr node) +: logger_(rclcpp::get_logger("rclcpp")), + clock_subscription_(nullptr), + ros_time_active_(false) +{ + this->attachNode(node); +} + +TimeSource::TimeSource() +: logger_(rclcpp::get_logger("rclcpp")), + ros_time_active_(false) +{ +} + +void TimeSource::attachNode(rclcpp::Node::SharedPtr node) +{ + attachNode( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + node->get_node_logging_interface(), + node->get_node_clock_interface(), + node->get_node_parameters_interface()); +} + +void TimeSource::attachNode( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) +{ + node_base_ = node_base_interface; + node_topics_ = node_topics_interface; + node_graph_ = node_graph_interface; + node_services_ = node_services_interface; + node_logging_ = node_logging_interface; + node_clock_ = node_clock_interface; + node_parameters_ = node_parameters_interface; + // TODO(tfoote): Update QOS + + logger_ = node_logging_->get_logger(); + + // Though this defaults to false, it can be overridden by initial parameter values for the node, + // which may be given by the user at the node's construction or even by command-line arguments. + rclcpp::ParameterValue use_sim_time_param; + const std::string use_sim_time_name = "use_sim_time"; + if (!node_parameters_->has_parameter(use_sim_time_name)) { + use_sim_time_param = node_parameters_->declare_parameter( + use_sim_time_name, + rclcpp::ParameterValue(false), + rcl_interfaces::msg::ParameterDescriptor()); + } else { + use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); + } + if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) { + if (use_sim_time_param.get()) { + parameter_state_ = SET_TRUE; + enable_ros_time(); + create_clock_sub(); + } + } else { + RCLCPP_ERROR( + logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", + rclcpp::to_string(use_sim_time_param.get_type()).c_str()); + } + sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback( + [use_sim_time_name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == use_sim_time_name && + parameter.get_type() != rclcpp::PARAMETER_BOOL) + { + result.successful = false; + result.reason = "'" + use_sim_time_name + "' must be a bool"; + break; + } + } + return result; + }); + + // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 + parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( + node_topics_, + std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1)); +} + +void TimeSource::detachNode() +{ + this->ros_time_active_ = false; + clock_subscription_.reset(); + parameter_subscription_.reset(); + node_base_.reset(); + node_topics_.reset(); + node_graph_.reset(); + node_services_.reset(); + node_logging_.reset(); + node_clock_.reset(); + if (sim_time_cb_handler_ && node_parameters_) { + node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get()); + } + sim_time_cb_handler_.reset(); + node_parameters_.reset(); + disable_ros_time(); +} + +void TimeSource::attachClock(std::shared_ptr clock) +{ + if (clock->get_clock_type() != RCL_ROS_TIME) { + throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock"); + } + + std::lock_guard guard(clock_list_lock_); + associated_clocks_.push_back(clock); + // Set the clock to zero unless there's a recently received message + auto time_msg = std::make_shared(); + if (last_msg_set_) { + time_msg = std::make_shared(last_msg_set_->clock); + } + set_clock(time_msg, ros_time_active_, clock); +} + +void TimeSource::detachClock(std::shared_ptr clock) +{ + std::lock_guard guard(clock_list_lock_); + auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); + if (result != associated_clocks_.end()) { + associated_clocks_.erase(result); + } else { + RCLCPP_ERROR(logger_, "failed to remove clock"); + } +} + +TimeSource::~TimeSource() +{ + if ( + node_base_ || node_topics_ || node_graph_ || node_services_ || + node_logging_ || node_clock_ || node_parameters_) + { + this->detachNode(); + } +} + +void TimeSource::set_clock( + const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, + std::shared_ptr clock) +{ + // Do change + if (!set_ros_time_enabled && clock->ros_time_is_active()) { + disable_ros_time(clock); + } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { + enable_ros_time(clock); + } + + auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), rclcpp::Time(*msg).nanoseconds()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to set ros_time_override_status"); + } +} + +void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg) +{ + if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) { + enable_ros_time(); + } + // Cache the last message in case a new clock is attached. + last_msg_set_ = msg; + auto time_msg = std::make_shared(msg->clock); + + if (SET_TRUE == this->parameter_state_) { + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + set_clock(time_msg, true, *it); + } + } +} + +void TimeSource::create_clock_sub() +{ + std::lock_guard guard(clock_sub_lock_); + if (clock_subscription_) { + // Subscription already created. + return; + } + + clock_subscription_ = rclcpp::create_subscription( + node_topics_, + "/clock", + rclcpp::QoS(QoSInitialization::from_rmw(rmw_qos_profile_default)), + std::bind(&TimeSource::clock_cb, this, std::placeholders::_1) + ); +} + +void TimeSource::destroy_clock_sub() +{ + std::lock_guard guard(clock_sub_lock_); + clock_subscription_.reset(); +} + +void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + // Filter out events on 'use_sim_time' parameter instances in other nodes. + if (event->node != node_base_->get_fully_qualified_name()) { + return; + } + // Filter for only 'use_sim_time' being added or changed. + rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::NEW, + rclcpp::ParameterEventsFilter::EventType::CHANGED}); + for (auto & it : filter.get_events()) { + if (it.second->value.type != ParameterType::PARAMETER_BOOL) { + RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); + continue; + } + if (it.second->value.bool_value) { + parameter_state_ = SET_TRUE; + enable_ros_time(); + create_clock_sub(); + } else { + parameter_state_ = SET_FALSE; + disable_ros_time(); + destroy_clock_sub(); + } + } + // Handle the case that use_sim_time was deleted. + rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::DELETED}); + for (auto & it : deleted.get_events()) { + (void) it; // if there is a match it's already matched, don't bother reading it. + // If the parameter is deleted mark it as unset but dont' change state. + parameter_state_ = UNSET; + } +} + +void TimeSource::enable_ros_time(std::shared_ptr clock) +{ + auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } +} + +void TimeSource::disable_ros_time(std::shared_ptr clock) +{ + auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } +} + +void TimeSource::enable_ros_time() +{ + if (ros_time_active_) { + // already enabled no-op + return; + } + + // Local storage + ros_time_active_ = true; + + // Update all attached clocks to zero or last recorded time + std::lock_guard guard(clock_list_lock_); + auto time_msg = std::make_shared(); + if (last_msg_set_) { + time_msg = std::make_shared(last_msg_set_->clock); + } + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + set_clock(time_msg, true, *it); + } +} + +void TimeSource::disable_ros_time() +{ + if (!ros_time_active_) { + // already disabled no-op + return; + } + + // Local storage + ros_time_active_ = false; + + // Update all attached clocks + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + auto msg = std::make_shared(); + set_clock(msg, false, *it); + } +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/timer.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/timer.cpp new file mode 100644 index 0000000..8291a12 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/timer.cpp @@ -0,0 +1,130 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/timer.hpp" + +#include +#include +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/exceptions.hpp" + +#include "rcutils/logging_macros.h" + +using rclcpp::TimerBase; + +TimerBase::TimerBase( + rclcpp::Clock::SharedPtr clock, + std::chrono::nanoseconds period, + rclcpp::Context::SharedPtr context) +: clock_(clock), timer_handle_(nullptr) +{ + if (nullptr == context) { + context = rclcpp::contexts::default_context::get_global_default_context(); + } + + auto rcl_context = context->get_rcl_context(); + + timer_handle_ = std::shared_ptr( + new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable + { + if (rcl_timer_fini(timer) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete timer; + // Captured shared pointers by copy, reset to make sure timer is finalized before clock + clock.reset(); + rcl_context.reset(); + }); + + *timer_handle_.get() = rcl_get_zero_initialized_timer(); + + rcl_clock_t * clock_handle = clock_->get_clock_handle(); + if ( + rcl_timer_init( + timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, + rcl_get_default_allocator()) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str); + rcl_reset_error(); + } +} + +TimerBase::~TimerBase() +{} + +void +TimerBase::cancel() +{ + if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) { + throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str); + } +} + +bool +TimerBase::is_canceled() +{ + bool is_canceled = false; + rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state"); + } + return is_canceled; +} + +void +TimerBase::reset() +{ + if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) { + throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str); + } +} + +bool +TimerBase::is_ready() +{ + bool ready = false; + if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) { + throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str); + } + return ready; +} + +std::chrono::nanoseconds +TimerBase::time_until_trigger() +{ + int64_t time_until_next_call = 0; + if ( + rcl_timer_get_time_until_next_call( + timer_handle_.get(), + &time_until_next_call) != RCL_RET_OK) + { + throw std::runtime_error( + std::string( + "Timer could not get time until next call: ") + rcl_get_error_string().str); + } + return std::chrono::nanoseconds(time_until_next_call); +} + +std::shared_ptr +TimerBase::get_timer_handle() +{ + return timer_handle_; +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/type_support.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/type_support.cpp new file mode 100644 index 0000000..27dfccb --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/type_support.cpp @@ -0,0 +1,116 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "rcl_interfaces/srv/describe_parameters.hpp" +#include "rcl_interfaces/srv/get_parameter_types.hpp" +#include "rcl_interfaces/srv/get_parameters.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" + +const rosidl_message_type_support_t * +rclcpp::type_support::get_intra_process_message_msg_type_support() +{ + return rosidl_typesupport_cpp::get_message_type_support_handle< + rcl_interfaces::msg::IntraProcessMessage + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_parameter_event_msg_type_support() +{ + return rosidl_typesupport_cpp::get_message_type_support_handle< + rcl_interfaces::msg::ParameterEvent + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_set_parameters_result_msg_type_support() +{ + return rosidl_typesupport_cpp::get_message_type_support_handle< + rcl_interfaces::msg::SetParametersResult + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_parameter_descriptor_msg_type_support() +{ + return rosidl_typesupport_cpp::get_message_type_support_handle< + rcl_interfaces::msg::ParameterDescriptor + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_list_parameters_result_msg_type_support() +{ + return rosidl_typesupport_cpp::get_message_type_support_handle< + rcl_interfaces::msg::ListParametersResult + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_get_parameters_srv_type_support() +{ + return rosidl_typesupport_cpp::get_service_type_support_handle< + rcl_interfaces::srv::GetParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_get_parameter_types_srv_type_support() +{ + return rosidl_typesupport_cpp::get_service_type_support_handle< + rcl_interfaces::srv::GetParameterTypes + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_set_parameters_srv_type_support() +{ + return rosidl_typesupport_cpp::get_service_type_support_handle< + rcl_interfaces::srv::SetParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_list_parameters_srv_type_support() +{ + return rosidl_typesupport_cpp::get_service_type_support_handle< + rcl_interfaces::srv::ListParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_describe_parameters_srv_type_support() +{ + return rosidl_typesupport_cpp::get_service_type_support_handle< + rcl_interfaces::srv::DescribeParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_set_parameters_atomically_srv_type_support() +{ + return rosidl_typesupport_cpp::get_service_type_support_handle< + rcl_interfaces::srv::SetParametersAtomically + >(); +} diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/utilities.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/utilities.cpp new file mode 100644 index 0000000..83008b4 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/utilities.cpp @@ -0,0 +1,188 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/utilities.hpp" + +#include +#include + +#include "./signal_handler.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/exceptions.hpp" + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" + +namespace rclcpp +{ + +void +init(int argc, char const * const argv[], const InitOptions & init_options) +{ + using contexts::default_context::get_global_default_context; + get_global_default_context()->init(argc, argv, init_options); + // Install the signal handlers. + install_signal_handlers(); +} + +bool +install_signal_handlers() +{ + return SignalHandler::get_global_signal_handler().install(); +} + +bool +signal_handlers_installed() +{ + return SignalHandler::get_global_signal_handler().is_installed(); +} + +bool +uninstall_signal_handlers() +{ + return SignalHandler::get_global_signal_handler().uninstall(); +} + +std::vector +init_and_remove_ros_arguments( + int argc, + char const * const argv[], + const InitOptions & init_options) +{ + init(argc, argv, init_options); + return remove_ros_arguments(argc, argv); +} + +std::vector +remove_ros_arguments(int argc, char const * const argv[]) +{ + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret; + + ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error(ret, "failed to parse arguments"); + } + + int nonros_argc = 0; + const char ** nonros_argv = NULL; + + ret = rcl_remove_ros_arguments( + argv, + &parsed_args, + alloc, + &nonros_argc, + &nonros_argv); + + if (RCL_RET_OK != ret || nonros_argc < 0) { + // Not using throw_from_rcl_error, because we may need to append deallocation failures. + exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state()); + rcl_reset_error(); + if (NULL != nonros_argv) { + alloc.deallocate(nonros_argv, alloc.state); + } + if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) { + base_exc.formatted_message += std::string( + ", failed also to cleanup parsed arguments, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw exceptions::RCLError(base_exc, ""); + } + + std::vector return_arguments(nonros_argc); + + for (int ii = 0; ii < nonros_argc; ++ii) { + return_arguments[ii] = std::string(nonros_argv[ii]); + } + + if (NULL != nonros_argv) { + alloc.deallocate(nonros_argv, alloc.state); + } + + ret = rcl_arguments_fini(&parsed_args); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error( + ret, "failed to cleanup parsed arguments, leaking memory"); + } + + return return_arguments; +} + +bool +ok(Context::SharedPtr context) +{ + using contexts::default_context::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); + } + return context->is_valid(); +} + +bool +is_initialized(Context::SharedPtr context) +{ + return ok(context); +} + +bool +shutdown(Context::SharedPtr context, const std::string & reason) +{ + using contexts::default_context::get_global_default_context; + auto default_context = get_global_default_context(); + if (nullptr == context) { + context = default_context; + } + bool ret = context->shutdown(reason); + if (context == default_context) { + uninstall_signal_handlers(); + } + return ret; +} + +void +on_shutdown(std::function callback, Context::SharedPtr context) +{ + using contexts::default_context::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); + } + context->on_shutdown(callback); +} + +bool +sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr context) +{ + using contexts::default_context::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); + } + return context->sleep_for(nanoseconds); +} + +const char * +get_c_string(const char * string_in) +{ + return string_in; +} + +const char * +get_c_string(const std::string & string_in) +{ + return string_in.c_str(); +} + +} // namespace rclcpp diff --git a/rclcpp-eloquent/rclcpp/src/rclcpp/waitable.cpp b/rclcpp-eloquent/rclcpp/src/rclcpp/waitable.cpp new file mode 100644 index 0000000..542b10a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/src/rclcpp/waitable.cpp @@ -0,0 +1,53 @@ +// Copyright 2018 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 "rclcpp/waitable.hpp" + +using rclcpp::Waitable; + +size_t +Waitable::get_number_of_ready_subscriptions() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_timers() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_clients() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_events() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_services() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_guard_conditions() +{ + return 0u; +} diff --git a/rclcpp-eloquent/rclcpp/test/executors/test_multi_threaded_executor.cpp b/rclcpp-eloquent/rclcpp/test/executors/test_multi_threaded_executor.cpp new file mode 100644 index 0000000..45cbe86 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/executors/test_multi_threaded_executor.cpp @@ -0,0 +1,102 @@ +// Copyright 2018 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +using namespace std::chrono_literals; + +using rcl_interfaces::msg::IntraProcessMessage; + +class TestMultiThreadedExecutor : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +/* + Test that timers are not taken multiple times when using reentrant callback groups. + */ +TEST_F(TestMultiThreadedExecutor, timer_over_take) { +#ifdef __linux__ + // This seems to be the most effective way to force the bug to happen on Linux. + // This is unnecessary on MacOS, since the default scheduler causes it. + struct sched_param param; + param.sched_priority = 0; + if (sched_setscheduler(0, SCHED_BATCH, ¶m) != 0) { + perror("sched_setscheduler"); + } +#endif + + bool yield_before_execute = true; + + rclcpp::executors::MultiThreadedExecutor executor( + rclcpp::executor::create_default_executor_arguments(), 2u, yield_before_execute); + + ASSERT_GT(executor.get_number_of_threads(), 1u); + + std::shared_ptr node = + std::make_shared("test_multi_threaded_executor_timer_over_take"); + + auto cbg = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant); + + rclcpp::Clock system_clock(RCL_STEADY_TIME); + std::mutex last_mutex; + auto last = system_clock.now(); + + std::atomic_int timer_count {0}; + + auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() { + // While this tolerance is a little wide, if the bug occurs, the next step will + // happen almost instantly. The purpose of this test is not to measure the jitter + // in timers, just assert that a reasonable amount of time has passed. + const double PERIOD = 0.1f; + const double TOLERANCE = 0.025f; + + rclcpp::Time now = system_clock.now(); + timer_count++; + + if (timer_count > 5) { + executor.cancel(); + } + + { + std::lock_guard lock(last_mutex); + double diff = std::abs((now - last).nanoseconds()) / 1.0e9; + last = now; + + if (diff < PERIOD - TOLERANCE) { + executor.cancel(); + ASSERT_GT(diff, PERIOD - TOLERANCE); + } + } + }; + + auto timer = node->create_wall_timer(100ms, timer_callback, cbg); + executor.add_node(node); + executor.spin(); +} diff --git a/rclcpp-eloquent/rclcpp/test/node_interfaces/node_wrapper.hpp b/rclcpp-eloquent/rclcpp/test/node_interfaces/node_wrapper.hpp new file mode 100644 index 0000000..f4319f5 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/node_interfaces/node_wrapper.hpp @@ -0,0 +1,64 @@ +// Copyright 2019 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 NODE_INTERFACES__NODE_WRAPPER_HPP_ +#define NODE_INTERFACES__NODE_WRAPPER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +class NodeWrapper +{ +public: + explicit NodeWrapper(const std::string & name) + : node(std::make_shared(name)) + {} + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() {return this->node->get_node_base_interface();} + + rclcpp::node_interfaces::NodeClockInterface::SharedPtr + get_node_clock_interface() {return this->node->get_node_clock_interface();} + + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr + get_node_graph_interface() {return this->node->get_node_graph_interface();} + + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface() {return this->node->get_node_logging_interface();} + + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr + get_node_timers_interface() {return this->node->get_node_timers_interface();} + + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface() {return this->node->get_node_topics_interface();} + + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface() {return this->node->get_node_services_interface();} + + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface() {return this->node->get_node_waitables_interface();} + + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + get_node_parameters_interface() {return this->node->get_node_parameters_interface();} + + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface() {return this->node->get_node_time_source_interface();} + +private: + rclcpp::Node::SharedPtr node; +}; + +#endif // NODE_INTERFACES__NODE_WRAPPER_HPP_ diff --git a/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp new file mode 100644 index 0000000..121f641 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +int main(void) +{ + auto node = std::make_shared("test_node"); + std::shared_ptr const_node_ptr = node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_ptr); + (void)result; +} diff --git a/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp new file mode 100644 index 0000000..4f5b831 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp @@ -0,0 +1,30 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +#include "../node_wrapper.hpp" + +int main(void) +{ + auto node = std::make_shared("test_wrapped_node"); + std::shared_ptr const_node_ptr = node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_ptr); + (void)result; +} diff --git a/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp new file mode 100644 index 0000000..ed14741 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +int main(void) +{ + auto node = std::make_shared("test_node"); + const rclcpp::Node & const_node_reference = *node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_reference); + (void)result; +} diff --git a/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp new file mode 100644 index 0000000..6d7c19c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp @@ -0,0 +1,30 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +#include "../node_wrapper.hpp" + +int main(void) +{ + auto node = std::make_shared("test_wrapped_node"); + const NodeWrapper & const_node_reference = *node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_reference); + (void)result; +} diff --git a/rclcpp-eloquent/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp new file mode 100644 index 0000000..015cade --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/node_interfaces/test_get_node_interfaces.cpp @@ -0,0 +1,117 @@ +// Copyright 2019 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 + +#include "gtest/gtest.h" + +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "./node_wrapper.hpp" + +static const std::string node_suffix = "test_get_node_interfaces"; // NOLINT + +class TestGetNodeInterfaces : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + node = std::make_shared(node_suffix); + wrapped_node = std::make_shared("wrapped_" + node_suffix); + } + + static void TearDownTestCase() + { + node.reset(); + wrapped_node.reset(); + } + + static rclcpp::Node::SharedPtr node; + static std::shared_ptr wrapped_node; +}; + +rclcpp::Node::SharedPtr TestGetNodeInterfaces::node = nullptr; +std::shared_ptr TestGetNodeInterfaces::wrapped_node = nullptr; + +TEST_F(TestGetNodeInterfaces, rclcpp_node_shared_ptr) { + auto result = rclcpp::node_interfaces::get_node_topics_interface(this->node); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, node_shared_ptr) { + auto result = rclcpp::node_interfaces::get_node_topics_interface(this->wrapped_node); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, rclcpp_node_reference) { + rclcpp::Node & node_reference = *this->node; + auto result = rclcpp::node_interfaces::get_node_topics_interface(node_reference); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, node_reference) { + NodeWrapper & wrapped_node_reference = *this->wrapped_node; + auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_reference); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, rclcpp_node_pointer) { + rclcpp::Node * node_pointer = this->node.get(); + auto result = rclcpp::node_interfaces::get_node_topics_interface(node_pointer); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, node_pointer) { + NodeWrapper * wrapped_node_pointer = this->wrapped_node.get(); + auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_pointer); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} + +TEST_F(TestGetNodeInterfaces, interface_shared_pointer) { + std::shared_ptr interface_shared_ptr = + this->node->get_node_topics_interface(); + auto result = rclcpp::node_interfaces::get_node_topics_interface(interface_shared_ptr); + static_assert( + std::is_same< + rclcpp::node_interfaces::NodeTopicsInterface *, + decltype(result) + >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *"); +} diff --git a/rclcpp-eloquent/rclcpp/test/resources/test_node/test_parameters.yaml b/rclcpp-eloquent/rclcpp/test/resources/test_node/test_parameters.yaml new file mode 100644 index 0000000..2c4e307 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/resources/test_node/test_parameters.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + parameter_int: 21 + parameter_string_array: [baz, baz, baz] diff --git a/rclcpp-eloquent/rclcpp/test/test_client.cpp b/rclcpp-eloquent/rclcpp/test/test_client.cpp new file mode 100644 index 0000000..fb3fefd --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_client.cpp @@ -0,0 +1,125 @@ +// Copyright 2017 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 +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +class TestClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestClientSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestClient, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = node->create_client("service"); + } + + { + ASSERT_THROW( + { + auto client = node->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestClient, construction_with_free_function) { + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "service", + rmw_qos_profile_services_default, + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?service", + rmw_qos_profile_services_default, + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +/* + Testing client construction and destruction for subnodes. + */ +TEST_F(TestClientSub, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = subnode->create_client("service"); + EXPECT_STREQ(client->get_service_name(), "/ns/sub_ns/service"); + } + + { + ASSERT_THROW( + { + auto client = node->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp-eloquent/rclcpp/test/test_create_timer.cpp b/rclcpp-eloquent/rclcpp/test/test_create_timer.cpp new file mode 100644 index 0000000..cff870a --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_create_timer.cpp @@ -0,0 +1,63 @@ +// Copyright 2019 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 +#include +#include + +#include "rclcpp/create_timer.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "node_interfaces/node_wrapper.hpp" + +using namespace std::chrono_literals; + +TEST(TestCreateTimer, timer_executes) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_create_timer_node"); + + std::atomic got_callback{false}; + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + [&got_callback, &timer]() { + got_callback = true; + timer->cancel(); + }); + + rclcpp::spin_some(node); + + ASSERT_TRUE(got_callback); + rclcpp::shutdown(); +} + +TEST(TestCreateTimer, call_with_node_wrapper_compiles) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timer_call_with_node_wrapper_compiles"); + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node.get_node_clock_interface()->get_clock(), + rclcpp::Duration(0ms), + []() {}); + rclcpp::shutdown(); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_duration.cpp b/rclcpp-eloquent/rclcpp/test/test_duration.cpp new file mode 100644 index 0000000..b77f046 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_duration.cpp @@ -0,0 +1,155 @@ +// Copyright 2017 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 +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/duration.hpp" + + +using namespace std::chrono_literals; + +class TestDuration : public ::testing::Test +{ +}; + +// TEST(TestDuration, conversions) { +// TODO(tfoote) Implement conversion methods +// } + +TEST(TestDuration, operators) { + rclcpp::Duration old(1, 0); + rclcpp::Duration young(2, 0); + + EXPECT_TRUE(old < young); + EXPECT_TRUE(young > old); + EXPECT_TRUE(old <= young); + EXPECT_TRUE(young >= old); + EXPECT_FALSE(young == old); + + rclcpp::Duration add = old + young; + EXPECT_EQ(add.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() + young.nanoseconds())); + EXPECT_EQ(add, old + young); + + rclcpp::Duration sub = young - old; + EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds())); + EXPECT_EQ(sub, young - old); + + rclcpp::Duration scale = old * 3; + EXPECT_EQ(scale.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() * 3)); + + rclcpp::Duration time = rclcpp::Duration(0, 0); + rclcpp::Duration copy_constructor_duration(time); + rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); + (void)assignment_op_duration; + assignment_op_duration = time; + + EXPECT_TRUE(time == copy_constructor_duration); + EXPECT_TRUE(time == assignment_op_duration); +} + +TEST(TestDuration, chrono_overloads) { + int64_t ns = 123456789l; + auto chrono_ns = std::chrono::nanoseconds(ns); + auto d1 = rclcpp::Duration(ns); + auto d2 = rclcpp::Duration(chrono_ns); + auto d3 = rclcpp::Duration(123456789ns); + EXPECT_EQ(d1, d2); + EXPECT_EQ(d1, d3); + EXPECT_EQ(d2, d3); + + // check non-nanosecond durations + std::chrono::milliseconds chrono_ms(100); + auto d4 = rclcpp::Duration(chrono_ms); + EXPECT_EQ(chrono_ms, d4.to_chrono()); + std::chrono::duration chrono_float_seconds(3.14); + auto d5 = rclcpp::Duration(chrono_float_seconds); + EXPECT_EQ(chrono_float_seconds, d5.to_chrono()); +} + +TEST(TestDuration, overflows) { + rclcpp::Duration max(std::numeric_limits::max()); + rclcpp::Duration min(std::numeric_limits::min()); + + rclcpp::Duration one(1); + rclcpp::Duration negative_one(-1); + + EXPECT_THROW(max + one, std::overflow_error); + EXPECT_THROW(min - one, std::underflow_error); + EXPECT_THROW(negative_one + min, std::underflow_error); + EXPECT_THROW(negative_one - max, std::underflow_error); + + rclcpp::Duration base_d = max * 0.3; + EXPECT_THROW(base_d * 4, std::overflow_error); + EXPECT_THROW(base_d * (-4), std::underflow_error); + + rclcpp::Duration base_d_neg = max * (-0.3); + EXPECT_THROW(base_d_neg * (-4), std::overflow_error); + EXPECT_THROW(base_d_neg * 4, std::underflow_error); +} + +TEST(TestDuration, negative_duration) { + rclcpp::Duration assignable_duration = rclcpp::Duration(0) - rclcpp::Duration(5, 0); + + { + // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146 + int64_t expected_value = -5000; + expected_value *= 1000 * 1000; + EXPECT_EQ(expected_value, assignable_duration.nanoseconds()); + } + + { + builtin_interfaces::msg::Duration duration_msg; + duration_msg.sec = -4; + duration_msg.nanosec = 250000000; + + assignable_duration = duration_msg; + // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146 + int64_t expected_value = -3750; + expected_value *= 1000 * 1000; + EXPECT_EQ(expected_value, assignable_duration.nanoseconds()); + } +} + +TEST(TestDuration, maximum_duration) { + rclcpp::Duration max_duration = rclcpp::Duration::max(); + rclcpp::Duration max(std::numeric_limits::max(), 999999999); + + EXPECT_EQ(max_duration, max); +} + +static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; +static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; + +TEST(TestDuration, from_seconds) { + EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0)); + EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0)); + EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(1.5)); + EXPECT_EQ(rclcpp::Duration(-ONE_AND_HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(-1.5)); +} + +TEST(TestDuration, std_chrono_constructors) { + EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0.0s)); + EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0s)); + EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s)); + EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s)); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_executor.cpp b/rclcpp-eloquent/rclcpp/test/test_executor.cpp new file mode 100644 index 0000000..9537141 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_executor.cpp @@ -0,0 +1,69 @@ +// Copyright 2017 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 +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +class TestExecutors : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +// Make sure that executors detach from nodes when destructing +TEST_F(TestExecutors, detachOnDestruction) { + { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + } + { + rclcpp::executors::SingleThreadedExecutor executor; + EXPECT_NO_THROW(executor.add_node(node)); + } +} + +// Make sure that the executor can automatically remove expired nodes correctly +TEST_F(TestExecutors, addTemporaryNode) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(std::make_shared("temporary_node")); + EXPECT_NO_THROW(executor.spin_some()); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_expand_topic_or_service_name.cpp b/rclcpp-eloquent/rclcpp/test/test_expand_topic_or_service_name.cpp new file mode 100644 index 0000000..98c1060 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_expand_topic_or_service_name.cpp @@ -0,0 +1,80 @@ +// Copyright 2017 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 "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" + +/* + Testing expand_topic_or_service_name. + */ +TEST(TestExpandTopicOrServiceName, normal) { + using rclcpp::expand_topic_or_service_name; + { + ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns")); + } +} + +/* + Testing exceptions of expand_topic_or_service_name. + */ +TEST(TestExpandTopicOrServiceName, exceptions) { + using rclcpp::expand_topic_or_service_name; + { + ASSERT_THROW( + { + expand_topic_or_service_name("chatter", "invalid_node?", "/ns"); + }, rclcpp::exceptions::InvalidNodeNameError); + } + + { + ASSERT_THROW( + { + expand_topic_or_service_name("chatter", "node", "/invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + + { + ASSERT_THROW( + { + expand_topic_or_service_name("chatter/42invalid", "node", "/ns"); + }, rclcpp::exceptions::InvalidTopicNameError); + } + + { + ASSERT_THROW( + { + // this one will only fail on the "full" topic name validation check + expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns"); + }, rclcpp::exceptions::InvalidTopicNameError); + } + + { + ASSERT_THROW( + { + // is_service = true + expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + // is_service = true + // this one will only fail on the "full" topic name validation check + expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp-eloquent/rclcpp/test/test_externally_defined_services.cpp b/rclcpp-eloquent/rclcpp/test/test_externally_defined_services.cpp new file mode 100644 index 0000000..563f7f6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_externally_defined_services.cpp @@ -0,0 +1,160 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl/service.h" + +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" + +class TestExternallyDefinedServices : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +void +callback( + const std::shared_ptr/*req*/, + std::shared_ptr/*resp*/) +{} + +TEST_F(TestExternallyDefinedServices, default_behavior) { + auto node_handle = rclcpp::Node::make_shared("base_node"); + + try { + auto srv = node_handle->create_service("test", callback); + } catch (const std::exception &) { + FAIL(); + return; + } + SUCCEED(); +} + +TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { + auto node_handle = rclcpp::Node::make_shared("base_node"); + + // mock for externally defined service + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + + rclcpp::AnyServiceCallback cb; + + // don't initialize the service + // expect fail + try { + rclcpp::Service( + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), + &service_handle, cb); + } catch (const std::runtime_error &) { + SUCCEED(); + return; + } + + FAIL(); +} + +TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { + auto node_handle = rclcpp::Node::make_shared("base_node"); + + // mock for externally defined service + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + const rosidl_service_type_support_t * ts = + rosidl_typesupport_cpp::get_service_type_support_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + + rclcpp::AnyServiceCallback cb; + + try { + rclcpp::Service( + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), + &service_handle, cb); + } catch (const std::runtime_error &) { + FAIL(); + return; + } + + // Destruct the service + ret = rcl_service_fini( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle()); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + + SUCCEED(); +} + +TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { + auto node_handle = rclcpp::Node::make_shared("base_node"); + + // mock for externally defined service + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + const rosidl_service_type_support_t * ts = + rosidl_typesupport_cpp::get_service_type_support_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + rclcpp::AnyServiceCallback cb; + + { + // Call constructor + rclcpp::Service srv_cpp( + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), + &service_handle, cb); + // Call destructor + } + + if (!service_handle.impl) { + FAIL(); + return; + } + + // Destruct the service + ret = rcl_service_fini( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle()); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + + SUCCEED(); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_find_weak_nodes.cpp b/rclcpp-eloquent/rclcpp/test/test_find_weak_nodes.cpp new file mode 100644 index 0000000..f4e58e8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_find_weak_nodes.cpp @@ -0,0 +1,82 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclcpp/strategies/allocator_memory_strategy.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestFindWeakNodes : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) { + // GIVEN + // A vector of weak pointers to nodes + auto memory_strategy = std::make_shared< + rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); + auto existing_node = rclcpp::Node::make_shared("existing_node"); + auto dead_node = rclcpp::Node::make_shared("dead_node"); + rclcpp::memory_strategy::MemoryStrategy::WeakNodeList weak_nodes; + weak_nodes.push_back(existing_node->get_node_base_interface()); + weak_nodes.push_back(dead_node->get_node_base_interface()); + + // AND + // Delete dead_node, creating a dangling pointer in weak_nodes + dead_node.reset(); + ASSERT_FALSE(weak_nodes.front().expired()); + ASSERT_TRUE(weak_nodes.back().expired()); + + // WHEN + bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes); + + // THEN + // The result of finding dangling node pointers should be true + ASSERT_TRUE(has_invalid_weak_nodes); + + // Prevent memory leak due to the order of destruction + memory_strategy->clear_handles(); +} + +TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) { + // GIVEN + // A vector of weak pointers to nodes, all valid + auto memory_strategy = std::make_shared< + rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); + auto existing_node1 = rclcpp::Node::make_shared("existing_node1"); + auto existing_node2 = rclcpp::Node::make_shared("existing_node2"); + rclcpp::memory_strategy::MemoryStrategy::WeakNodeList weak_nodes; + weak_nodes.push_back(existing_node1->get_node_base_interface()); + weak_nodes.push_back(existing_node2->get_node_base_interface()); + ASSERT_FALSE(weak_nodes.front().expired()); + ASSERT_FALSE(weak_nodes.back().expired()); + + // WHEN + bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes); + + // THEN + // The result of finding dangling node pointers should be false + ASSERT_FALSE(has_invalid_weak_nodes); + + // Prevent memory leak due to the order of destruction + memory_strategy->clear_handles(); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_function_traits.cpp b/rclcpp-eloquent/rclcpp/test/test_function_traits.cpp new file mode 100644 index 0000000..a9e6e8d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_function_traits.cpp @@ -0,0 +1,753 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/function_traits.hpp" + +int func_no_args() +{ + return 0; +} + +// NOLINTNEXTLINE(readability/casting) +int func_one_int(int) +{ + return 1; +} + +int func_two_ints(int, int) +{ + return 2; +} + +int func_one_int_one_char(int, char) +{ + return 3; +} + +struct FunctionObjectNoArgs +{ + int operator()() const + { + return 0; + } +}; + +struct FunctionObjectOneInt +{ + int operator()(int) const + { + return 1; + } +}; + +struct FunctionObjectTwoInts +{ + int operator()(int, int) const + { + return 2; + } +}; + +struct FunctionObjectOneIntOneChar +{ + int operator()(int, char) const + { + return 3; + } +}; + +struct ObjectMember +{ + int callback_one_bool(bool a) + { + (void)a; + return 7; + } + + int callback_one_bool_const(bool a) const + { + (void)a; + return 7; + } + + int callback_two_bools(bool a, bool b) + { + (void)a; + (void)b; + return 8; + } + + int callback_one_bool_one_float(bool a, float b) + { + (void)a; + (void)b; + return 9; + } +}; + +template< + typename FunctorT, + std::size_t Arity = 0, + typename std::enable_if< + rclcpp::function_traits::arity_comparator::value>::type * = nullptr +> +int func_accept_callback(FunctorT callback) +{ + return callback(); +} + +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::check_arguments::value + >::type * = nullptr +> +int func_accept_callback(FunctorT callback) +{ + int a = 4; + return callback(a); +} + +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::check_arguments::value + >::type * = nullptr +> +int func_accept_callback(FunctorT callback) +{ + int a = 5; + int b = 6; + return callback(a, b); +} + +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::check_arguments::value + >::type * = nullptr +> +int func_accept_callback(FunctorT callback) +{ + int a = 7; + char b = 8; + return callback(a, b); +} + +template< + typename FunctorT, + std::size_t Arity = 0, + typename std::enable_if< + rclcpp::function_traits::arity_comparator::value + >::type * = nullptr, + typename std::enable_if< + std::is_same< + typename rclcpp::function_traits::function_traits::return_type, + double + >::value + >::type * = nullptr +> +double func_accept_callback_return_type(FunctorT callback) +{ + return callback(); +} + +template< + typename FunctorT, + std::size_t Arity = 0, + typename std::enable_if< + rclcpp::function_traits::arity_comparator::value + >::type * = nullptr, + typename std::enable_if< + std::is_same< + typename rclcpp::function_traits::function_traits::return_type, + std::string + >::value + >::type * = nullptr +> +std::string func_accept_callback_return_type(FunctorT callback) +{ + return callback(); +} + +/* + Tests that funcion_traits calculates arity of several functors. + */ +TEST(TestFunctionTraits, arity) { + // Test regular functions + static_assert( + rclcpp::function_traits::function_traits::arity == 0, + "Functor does not accept arguments"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 1, + "Functor only accepts one argument"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 2, + "Functor only accepts two arguments"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 2, + "Functor only accepts two arguments"); + + // Test lambdas + auto lambda_no_args = []() { + return 0; + }; + + auto lambda_one_int = [](int one) { + (void)one; + return 1; + }; + + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; + return 2; + }; + + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; + return 3; + }; + + static_assert( + rclcpp::function_traits::function_traits::arity == 0, + "Functor does not accept arguments"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 1, + "Functor only accepts one argument"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 2, + "Functor only accepts two arguments"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 2, + "Functor only accepts two arguments"); + + // Test objects that have a call operator + static_assert( + rclcpp::function_traits::function_traits::arity == 0, + "Functor does not accept arguments"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 1, + "Functor only accepts one argument"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 2, + "Functor only accepts two arguments"); + + static_assert( + rclcpp::function_traits::function_traits::arity == 2, + "Functor only accepts two arguments"); +} + +/* + Tests that funcion_traits deducts the type of the arguments of several functors. + */ +TEST(TestFunctionTraits, argument_types) { + // Test regular functions + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<1> + >::value, "Functor accepts an int as second argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits< + decltype(func_one_int_one_char) + >::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + char, + rclcpp::function_traits::function_traits< + decltype(func_one_int_one_char) + >::template argument_type<1> + >::value, "Functor accepts a char as second argument"); + + // Test lambdas + auto lambda_one_int = [](int one) { + (void)one; + return 1; + }; + + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; + return 2; + }; + + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; + return 3; + }; + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<1> + >::value, "Functor accepts an int as second argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits< + decltype(lambda_one_int_one_char) + >::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + char, + rclcpp::function_traits::function_traits< + decltype(lambda_one_int_one_char) + >::template argument_type<1> + >::value, "Functor accepts a char as second argument"); + + // Test objects that have a call operator + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<1> + >::value, "Functor accepts an int as second argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits< + FunctionObjectOneIntOneChar + >::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + char, + rclcpp::function_traits::function_traits< + FunctionObjectOneIntOneChar + >::template argument_type<1> + >::value, "Functor accepts a char as second argument"); + + ObjectMember object_member; + + auto bind_one_bool = std::bind( + &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + + static_assert( + std::is_same< + bool, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts a bool as first argument"); + + auto bind_one_bool_const = std::bind( + &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + + static_assert( + std::is_same< + bool, + rclcpp::function_traits::function_traits::template + argument_type<0> + >::value, "Functor accepts a bool as first argument"); + + auto bind_two_bools = std::bind( + &ObjectMember::callback_two_bools, &object_member, std::placeholders::_1, + std::placeholders::_2); + + static_assert( + std::is_same< + bool, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts a bool as first argument"); + + static_assert( + std::is_same< + bool, + rclcpp::function_traits::function_traits::template argument_type<1> + >::value, "Functor accepts a bool as second argument"); + + auto bind_one_bool_one_float = std::bind( + &ObjectMember::callback_one_bool_one_float, &object_member, std::placeholders::_1, + std::placeholders::_2); + + static_assert( + std::is_same< + bool, + rclcpp::function_traits::function_traits< + decltype(bind_one_bool_one_float) + >::template argument_type<0> + >::value, "Functor accepts a bool as first argument"); + + static_assert( + std::is_same< + float, + rclcpp::function_traits::function_traits< + decltype(bind_one_bool_one_float) + >::template argument_type<1> + >::value, "Functor accepts a float as second argument"); + + auto bind_one_int = std::bind(func_one_int, std::placeholders::_1); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + auto bind_two_ints = std::bind(func_two_ints, std::placeholders::_1, std::placeholders::_2); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits::template argument_type<1> + >::value, "Functor accepts an int as second argument"); + + auto bind_one_int_one_char = std::bind( + func_one_int_one_char, std::placeholders::_1, std::placeholders::_2); + + static_assert( + std::is_same< + int, + rclcpp::function_traits::function_traits< + decltype(bind_one_int_one_char) + >::template argument_type<0> + >::value, "Functor accepts an int as first argument"); + + static_assert( + std::is_same< + char, + rclcpp::function_traits::function_traits< + decltype(bind_one_int_one_char) + >::template argument_type<1> + >::value, "Functor accepts a char as second argument"); +} + +/* + Tests that funcion_traits checks the types of the arguments of several functors. + */ +TEST(TestFunctionTraits, check_arguments) { + // Test regular functions + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts a single int as arguments"); + + static_assert( + !rclcpp::function_traits::check_arguments::value, + "Functor does not accept a char as argument"); + + static_assert( + !rclcpp::function_traits::check_arguments::value, + "Functor does not accept two arguments"); + + static_assert( + !rclcpp::function_traits::check_arguments::value, + "Functor does not accept a single int as argument, requires two ints"); + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts two ints as arguments"); + + static_assert( + !rclcpp::function_traits::check_arguments::value, + "Functor does not accept a bool and an int as arguments, requires two ints"); + + static_assert( + !rclcpp::function_traits::check_arguments::value, + "Functor does not accept an int and a char as arguments, requires two ints"); + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts an int and a char as arguments"); + + // Test lambdas + auto lambda_one_int = [](int one) { + (void)one; + return 1; + }; + + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; + return 2; + }; + + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; + return 3; + }; + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts an int as the only argument"); + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts two ints as arguments"); + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts an int and a char as arguments"); + + // Test objects that have a call operator + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts an int as the only argument"); + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts two ints as arguments"); + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts an int and a char as arguments"); + + ObjectMember object_member; + + auto bind_one_bool = std::bind( + &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + + // Test std::bind functions + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts a single bool as arguments"); + + auto bind_one_bool_const = std::bind( + &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + + // Test std::bind functions + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts a single bool as arguments"); +} + +/* + Tests that same_arguments work. +*/ +TEST(TestFunctionTraits, same_arguments) { + auto lambda_one_int = [](int one) { + (void)one; + return 1; + }; + + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; + return 1; + }; + + static_assert( + rclcpp::function_traits::same_arguments< + decltype(lambda_one_int), decltype(func_one_int) + >::value, + "Lambda and function have the same arguments"); + + static_assert( + !rclcpp::function_traits::same_arguments< + decltype(lambda_two_ints), decltype(func_one_int) + >::value, + "Lambda and function have different arguments"); + + static_assert( + !rclcpp::function_traits::same_arguments< + decltype(func_one_int_one_char), decltype(func_two_ints) + >::value, + "Functions have different arguments"); + + static_assert( + !rclcpp::function_traits::same_arguments< + decltype(lambda_one_int), decltype(lambda_two_ints) + >::value, + "Lambdas have different arguments"); + + static_assert( + rclcpp::function_traits::same_arguments::value, + "Functor and function have the same arguments"); + + static_assert( + rclcpp::function_traits::same_arguments< + FunctionObjectTwoInts, decltype(lambda_two_ints)>::value, + "Functor and lambda have the same arguments"); +} + +TEST(TestFunctionTraits, return_type) { + // Test regular function + static_assert( + std::is_same< + rclcpp::function_traits::function_traits::return_type, + int + >::value, + "Functor return ints"); + + // Test lambda + auto lambda_one_int_return_double = [](int one) -> double { + (void)one; + return 1.0; + }; + + static_assert( + std::is_same< + rclcpp::function_traits::function_traits< + decltype(lambda_one_int_return_double) + >::return_type, + double + >::value, + "Lambda returns a double"); + + // Test objects that have a call operator + static_assert( + std::is_same< + rclcpp::function_traits::function_traits::return_type, + int + >::value, + "Functor return ints"); +} + +/* + Tests that functions are matched via SFINAE. + */ +TEST(TestFunctionTraits, sfinae_match) { + EXPECT_EQ(0, func_accept_callback(func_no_args)); + + EXPECT_EQ(1, func_accept_callback(func_one_int)); + + EXPECT_EQ(2, func_accept_callback(func_two_ints)); + + EXPECT_EQ(3, func_accept_callback(func_one_int_one_char)); + + auto lambda_no_args = []() { + return 0; + }; + + auto lambda_one_int = [](int one) { + (void)one; + return 1; + }; + + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; + return 2; + }; + + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; + return 3; + }; + + EXPECT_EQ(0, func_accept_callback(lambda_no_args)); + + EXPECT_EQ(1, func_accept_callback(lambda_one_int)); + + EXPECT_EQ(2, func_accept_callback(lambda_two_ints)); + + EXPECT_EQ(3, func_accept_callback(lambda_one_int_one_char)); + + EXPECT_EQ(0, func_accept_callback(FunctionObjectNoArgs())); + + EXPECT_EQ(1, func_accept_callback(FunctionObjectOneInt())); + + EXPECT_EQ(2, func_accept_callback(FunctionObjectTwoInts())); + + EXPECT_EQ(3, func_accept_callback(FunctionObjectOneIntOneChar())); + + auto lambda_no_args_double = []() -> double { + return 123.45; + }; + + auto lambda_no_args_string = []() -> std::string { + return std::string("foo"); + }; + + EXPECT_EQ(123.45, func_accept_callback_return_type(lambda_no_args_double)); + + EXPECT_EQ("foo", func_accept_callback_return_type(lambda_no_args_string)); +} + +class TestMember : public ::testing::Test +{ +public: + void MemberFunctor(int, float, std::string) {} +}; + +/* + Regression test for https://github.com/ros2/rclcpp/issues/479, specific to classes using the + TEST_F GTest macro. +*/ +TEST_F(TestMember, bind_member_functor) { + auto bind_member_functor = std::bind( + &TestMember::MemberFunctor, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts an int, a float and a string as arguments"); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_init.cpp b/rclcpp-eloquent/rclcpp/test/test_init.cpp new file mode 100644 index 0000000..7855499 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_init.cpp @@ -0,0 +1,29 @@ +// Copyright 2018 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 "rclcpp/utilities.hpp" + +TEST(TestInit, is_initialized) { + EXPECT_FALSE(rclcpp::is_initialized()); + + rclcpp::init(0, nullptr); + + EXPECT_TRUE(rclcpp::is_initialized()); + + rclcpp::shutdown(); + + EXPECT_FALSE(rclcpp::is_initialized()); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_intra_process_buffer.cpp b/rclcpp-eloquent/rclcpp/test/test_intra_process_buffer.cpp new file mode 100644 index 0000000..87d2bae --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_intra_process_buffer.cpp @@ -0,0 +1,240 @@ +// Copyright 2019 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 + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" + +/* + Construtctor + */ +TEST(TestIntraProcessBuffer, constructor) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto shared_buffer_impl = + std::make_unique>(2); + + SharedIntraProcessBufferT shared_intra_process_buffer(std::move(shared_buffer_impl)); + + EXPECT_EQ(true, shared_intra_process_buffer.use_take_shared_method()); + + auto unique_buffer_impl = + std::make_unique>(2); + + UniqueIntraProcessBufferT unique_intra_process_buffer(std::move(unique_buffer_impl)); + + EXPECT_EQ(false, unique_intra_process_buffer.use_take_shared_method()); +} + +/* + Add data to an intra-process buffer with an implementations that stores shared_ptr + Messages are extracted using the same data as the implementation, i.e. shared_ptr + - Add shared_ptr no copies are expected + - Add unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, shared_buffer_add) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(2u, original_shared_msg.use_count()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_shared_msg.use_count(), popped_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + auto original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + popped_shared_msg = intra_process_buffer.consume_shared(); + popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(1u, popped_shared_msg.use_count()); + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} + +/* + Add data to an intra-process buffer with an implementations that stores unique_ptr + Messages are extracted using the same data as the implementation, i.e. unique_ptr + - Add shared_ptr a copy is expected + - Add unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, unique_buffer_add) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(1u, original_shared_msg.use_count()); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + auto popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(*original_shared_msg, *popped_unique_msg); + EXPECT_NE(original_message_pointer, popped_message_pointer); + + auto original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} + +/* + Consume data from an intra-process buffer with an implementations that stores shared_ptr + Messages are inserted using the same data as the implementation, i.e. shared_ptr + - Request shared_ptr no copies are expected + - Request unique_ptr a copy is expected + */ +TEST(TestIntraProcessBuffer, shared_buffer_consume) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(2u, original_shared_msg.use_count()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_shared_msg.use_count(), popped_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_shared_msg = std::make_shared('b'); + original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(1u, original_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_unique_msg); + EXPECT_NE(original_message_pointer, popped_message_pointer); +} + +/* + Consume data from an intra-process buffer with an implementations that stores unique_ptr + Messages are inserted using the same data as the implementation, i.e. unique_ptr + - Request shared_ptr no copies are expected + - Request unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, unique_buffer_consume) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_unique_msg = std::make_unique('a'); + auto original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_intra_process_manager.cpp b/rclcpp-eloquent/rclcpp/test/test_intra_process_manager.cpp new file mode 100644 index 0000000..b71fb1d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_intra_process_manager.cpp @@ -0,0 +1,672 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#define RCLCPP_BUILDING_LIBRARY 1 +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" +#include "rmw/types.h" +#include "rmw/qos_profiles.h" + +// NOLINTNEXTLINE(build/include_order) +#include + +namespace rclcpp +{ +// forward declaration +namespace experimental +{ +class IntraProcessManager; +} // namespace experimental + +namespace mock +{ + +using IntraProcessManagerSharedPtr = + std::shared_ptr; + +using IntraProcessManagerWeakPtr = + std::weak_ptr; + +class PublisherBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) + + PublisherBase() + : qos(rclcpp::QoS(10)), + topic_name("topic") + {} + + virtual ~PublisherBase() + {} + + const char * get_topic_name() const + { + return topic_name.c_str(); + } + + void set_intra_process_manager( + uint64_t intra_process_publisher_id, + IntraProcessManagerSharedPtr ipm) + { + intra_process_publisher_id_ = intra_process_publisher_id; + weak_ipm_ = ipm; + } + + rclcpp::QoS + get_actual_qos() const + { + return qos; + } + + bool + operator==(const rmw_gid_t & gid) const + { + (void)gid; + return false; + } + + bool + operator==(const rmw_gid_t * gid) const + { + (void)gid; + return false; + } + + rclcpp::QoS qos; + std::string topic_name; + uint64_t intra_process_publisher_id_; + IntraProcessManagerWeakPtr weak_ipm_; +}; + +template> +class Publisher : public PublisherBase +{ +public: + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) + + Publisher() + { + qos = rclcpp::QoS(10); + auto allocator = std::make_shared(); + message_allocator_ = std::make_shared(*allocator.get()); + } + + // The following functions use the IntraProcessManager + // so they are declared after including it to avoid "invalid use of incomplete type" + void publish(MessageUniquePtr msg); + + std::shared_ptr message_allocator_; +}; + +} // namespace mock +} // namespace rclcpp + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ +namespace mock +{ +template +class IntraProcessBuffer +{ +public: + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) + + IntraProcessBuffer() + {} + + void add(ConstMessageSharedPtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + shared_msg = msg; + } + + void add(MessageUniquePtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + unique_msg = std::move(msg); + } + + void pop(std::uintptr_t & msg_ptr) + { + msg_ptr = message_ptr; + message_ptr = 0; + } + + // need to store the messages somewhere otherwise the memory address will be reused + ConstMessageSharedPtr shared_msg; + MessageUniquePtr unique_msg; + + std::uintptr_t message_ptr; +}; + +} // namespace mock +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + + +namespace rclcpp +{ +namespace experimental +{ +namespace mock +{ + +class SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + + SubscriptionIntraProcessBase() + : qos_profile(rmw_qos_profile_default), topic_name("topic") + {} + + virtual ~SubscriptionIntraProcessBase() {} + + virtual bool + use_take_shared_method() const = 0; + + rmw_qos_profile_t + get_actual_qos() + { + return qos_profile; + } + + const char * + get_topic_name() + { + return topic_name; + } + + rmw_qos_profile_t qos_profile; + const char * topic_name; +}; + +template +class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + SubscriptionIntraProcess() + : take_shared_method(false) + { + buffer = std::make_unique>(); + } + + void + provide_intra_process_message(std::shared_ptr msg) + { + buffer->add(msg); + } + + void + provide_intra_process_message(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } + + std::uintptr_t + pop() + { + std::uintptr_t ptr; + buffer->pop(ptr); + return ptr; + } + + bool + use_take_shared_method() const + { + return take_shared_method; + } + + bool take_shared_method; + + typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::UniquePtr buffer; +}; + +} // namespace mock +} // namespace experimental +} // namespace rclcpp + +// Prevent the header files of the mocked classes to be included +#define RCLCPP__PUBLISHER_HPP_ +#define RCLCPP__PUBLISHER_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +// Force ipm to use our mock publisher class. +#define Publisher mock::Publisher +#define PublisherBase mock::PublisherBase +#define IntraProcessBuffer mock::IntraProcessBuffer +#define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase +#define SubscriptionIntraProcess mock::SubscriptionIntraProcess +#include "../src/rclcpp/intra_process_manager.cpp" +#undef Publisher +#undef PublisherBase +#undef IntraProcessBuffer +#undef SubscriptionIntraProcessBase +#undef SubscriptionIntraProcess + +using ::testing::_; +using ::testing::UnorderedElementsAre; + +namespace rclcpp +{ +namespace mock +{ + +template +void Publisher::publish(MessageUniquePtr msg) +{ + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + message_allocator_); +} + +} // namespace mock +} // namespace rclcpp + +/* + This tests how the class connects and disconnects publishers and subscriptions: + - Creates 2 publishers on different topics and a subscription to one of them. + Add everything to the intra-process manager. + - All the entities are expected to have different ids. + - Check the subscriptions count for each publisher. + - One of the publishers is expected to have 1 subscription, while the other 0. + - Check the subscription count for a non existing publisher id, should return 0. + - Add a new publisher and a new subscription both with reliable QoS. + - The subscriptions count of the previous publisher is expected to remain unchanged, + while the new publisher is expected to have 2 subscriptions (it's compatible with both QoS). + - Remove the just added subscriptions. + - The count for the last publisher is expected to decrease to 1. + */ +TEST(TestIntraProcessManager, add_pub_sub) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + p1->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + + auto p2 = std::make_shared(); + p2->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + p2->topic_name = "different_topic_name"; + + auto s1 = std::make_shared(); + s1->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + + auto p1_id = ipm->add_publisher(p1); + auto p2_id = ipm->add_publisher(p2); + auto s1_id = ipm->add_subscription(s1); + + bool unique_ids = p1_id != p2_id && p2_id != s1_id; + ASSERT_TRUE(unique_ids); + + size_t p1_subs = ipm->get_subscription_count(p1_id); + size_t p2_subs = ipm->get_subscription_count(p2_id); + size_t non_existing_pub_subs = ipm->get_subscription_count(42); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(0u, non_existing_pub_subs); + + auto p3 = std::make_shared(); + p3->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + auto s2 = std::make_shared(); + s2->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + auto s2_id = ipm->add_subscription(s2); + auto p3_id = ipm->add_publisher(p3); + + p1_subs = ipm->get_subscription_count(p1_id); + p2_subs = ipm->get_subscription_count(p2_id); + size_t p3_subs = ipm->get_subscription_count(p3_id); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(2u, p3_subs); + + ipm->remove_subscription(s2_id); + p1_subs = ipm->get_subscription_count(p1_id); + p2_subs = ipm->get_subscription_count(p2_id); + p3_subs = ipm->get_subscription_count(p3_id); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(1u, p3_subs); +} + +/* + This tests the minimal usage of the class where there is a single subscription per publisher: + - Publishes a unique_ptr message with a subscription requesting ownership. + - The received message is expected to be the same. + - Remove the first subscription from ipm and add a new one. + - Publishes a unique_ptr message with a subscription not requesting ownership. + - The received message is expected to be the same, the first subscription do not receive it. + - Publishes a shared_ptr message with a subscription not requesting ownership. + - The received message is expected to be the same. + */ +TEST(TestIntraProcessManager, single_subscription) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared(); + s1->take_shared_method = false; + auto s1_id = ipm->add_subscription(s1); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_1 = s1->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_1); + + ipm->remove_subscription(s1_id); + auto s2 = std::make_shared(); + s2->take_shared_method = true; + auto s2_id = ipm->add_subscription(s2); + (void)s2_id; + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); + ASSERT_EQ(0u, received_message_pointer_1); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + received_message_pointer_2 = s2->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); +} + +/* + This tests the usage of the class where there are multiple subscriptions of the same type: + - Publishes a unique_ptr message with 2 subscriptions requesting ownership. + - One is expected to receive the published message, while the other will receive a copy. + - Publishes a unique_ptr message with 2 subscriptions not requesting ownership. + - Both received messages are expected to be the same as the published one. + - Publishes a shared_ptr message with 2 subscriptions requesting ownership. + - Both received messages are expected to be a copy of the published one. + - Publishes a shared_ptr message with 2 subscriptions not requesting ownership. + - Both received messages are expected to be the same as the published one. + */ +TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared(); + s1->take_shared_method = false; + auto s1_id = ipm->add_subscription(s1); + + auto s2 = std::make_shared(); + s2->take_shared_method = false; + auto s2_id = ipm->add_subscription(s2); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + bool received_original_1 = s1->pop() == original_message_pointer; + bool received_original_2 = s2->pop() == original_message_pointer; + std::vector received_original_vec = + {received_original_1, received_original_2}; + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false)); + + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); + + auto s3 = std::make_shared(); + s3->take_shared_method = true; + auto s3_id = ipm->add_subscription(s3); + + auto s4 = std::make_shared(); + s4->take_shared_method = true; + auto s4_id = ipm->add_subscription(s4); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_3 = s3->pop(); + auto received_message_pointer_4 = s4->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_3); + ASSERT_EQ(original_message_pointer, received_message_pointer_4); + + ipm->remove_subscription(s3_id); + ipm->remove_subscription(s4_id); + + auto s5 = std::make_shared(); + s5->take_shared_method = false; + auto s5_id = ipm->add_subscription(s5); + + auto s6 = std::make_shared(); + s6->take_shared_method = false; + auto s6_id = ipm->add_subscription(s6); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_5 = s5->pop(); + auto received_message_pointer_6 = s6->pop(); + ASSERT_NE(original_message_pointer, received_message_pointer_5); + // Someone gets the original unique_ptr, the last one to take. + ASSERT_EQ(original_message_pointer, received_message_pointer_6); + + ipm->remove_subscription(s5_id); + ipm->remove_subscription(s6_id); + + auto s7 = std::make_shared(); + s7->take_shared_method = true; + auto s7_id = ipm->add_subscription(s7); + (void)s7_id; + + auto s8 = std::make_shared(); + s8->take_shared_method = true; + auto s8_id = ipm->add_subscription(s8); + (void)s8_id; + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_7 = s7->pop(); + auto received_message_pointer_8 = s8->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_7); + ASSERT_EQ(original_message_pointer, received_message_pointer_8); +} + +/* + This tests the usage of the class where there are multiple subscriptions of different types: + - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not. + - The one requesting ownership is expected to receive the published message, + while the other is expected to receive a copy. + - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 1 not. + - One of the subscriptions requesting ownership is expected to receive the published message, + while both other subscriptions are expected to receive different copies. + - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 2 not. + - The 2 subscriptions not requesting ownership are expected to both receive the same copy + of the message, one of the subscription requesting ownership is expected to receive a + different copy, while the last is expected to receive the published message. + - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not. + - The subscription requesting ownership is expected to receive a copy of the message, while + the other is expected to receive the published message + */ +TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared(); + s1->take_shared_method = true; + auto s1_id = ipm->add_subscription(s1); + + auto s2 = std::make_shared(); + s2->take_shared_method = false; + auto s2_id = ipm->add_subscription(s2); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + ASSERT_NE(original_message_pointer, received_message_pointer_1); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); + + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); + + auto s3 = std::make_shared(); + s3->take_shared_method = false; + auto s3_id = ipm->add_subscription(s3); + + auto s4 = std::make_shared(); + s4->take_shared_method = false; + auto s4_id = ipm->add_subscription(s4); + + auto s5 = std::make_shared(); + s5->take_shared_method = true; + auto s5_id = ipm->add_subscription(s5); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_3 = s3->pop(); + auto received_message_pointer_4 = s4->pop(); + auto received_message_pointer_5 = s5->pop(); + bool received_original_3 = received_message_pointer_3 == original_message_pointer; + bool received_original_4 = received_message_pointer_4 == original_message_pointer; + bool received_original_5 = received_message_pointer_5 == original_message_pointer; + std::vector received_original_vec = + {received_original_3, received_original_4, received_original_5}; + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false, false)); + ASSERT_NE(received_message_pointer_3, received_message_pointer_4); + ASSERT_NE(received_message_pointer_5, received_message_pointer_3); + ASSERT_NE(received_message_pointer_5, received_message_pointer_4); + + ipm->remove_subscription(s3_id); + ipm->remove_subscription(s4_id); + ipm->remove_subscription(s5_id); + + auto s6 = std::make_shared(); + s6->take_shared_method = true; + auto s6_id = ipm->add_subscription(s6); + + auto s7 = std::make_shared(); + s7->take_shared_method = true; + auto s7_id = ipm->add_subscription(s7); + + auto s8 = std::make_shared(); + s8->take_shared_method = false; + auto s8_id = ipm->add_subscription(s8); + + auto s9 = std::make_shared(); + s9->take_shared_method = false; + auto s9_id = ipm->add_subscription(s9); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_6 = s6->pop(); + auto received_message_pointer_7 = s7->pop(); + auto received_message_pointer_8 = s8->pop(); + auto received_message_pointer_9 = s9->pop(); + bool received_original_8 = received_message_pointer_8 == original_message_pointer; + bool received_original_9 = received_message_pointer_9 == original_message_pointer; + received_original_vec = {received_original_8, received_original_9}; + ASSERT_EQ(received_message_pointer_6, received_message_pointer_7); + ASSERT_NE(original_message_pointer, received_message_pointer_6); + ASSERT_NE(original_message_pointer, received_message_pointer_7); + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false)); + ASSERT_NE(received_message_pointer_8, received_message_pointer_6); + ASSERT_NE(received_message_pointer_9, received_message_pointer_6); + + ipm->remove_subscription(s6_id); + ipm->remove_subscription(s7_id); + ipm->remove_subscription(s8_id); + ipm->remove_subscription(s9_id); + + auto s10 = std::make_shared(); + s10->take_shared_method = false; + auto s10_id = ipm->add_subscription(s10); + (void)s10_id; + + auto s11 = std::make_shared(); + s11->take_shared_method = true; + auto s11_id = ipm->add_subscription(s11); + (void)s11_id; + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_10 = s10->pop(); + auto received_message_pointer_11 = s11->pop(); + EXPECT_EQ(original_message_pointer, received_message_pointer_10); + EXPECT_NE(original_message_pointer, received_message_pointer_11); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_loaned_message.cpp b/rclcpp-eloquent/rclcpp/test/test_loaned_message.cpp new file mode 100644 index 0000000..651b367 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_loaned_message.cpp @@ -0,0 +1,83 @@ +// Copyright 2019 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 + +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/basic_types.hpp" + +using MessageT = test_msgs::msg::BasicTypes; +using LoanedMessageT = rclcpp::LoanedMessage; + +class TestLoanedMessage : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestLoanedMessage, initialize) { + auto node = std::make_shared("loaned_message_test_node"); + auto pub = node->create_publisher("loaned_message_test_topic", 1); + + auto loaned_msg = rclcpp::LoanedMessage(pub.get(), pub->get_allocator()); + ASSERT_TRUE(loaned_msg.is_valid()); + loaned_msg.get().float32_value = 42.0f; + ASSERT_EQ(42.0f, loaned_msg.get().float32_value); + + SUCCEED(); +} + +TEST_F(TestLoanedMessage, loan_from_pub) { + auto node = std::make_shared("loaned_message_test_node"); + auto pub = node->create_publisher("loaned_message_test_topic", 1); + + auto loaned_msg = pub->borrow_loaned_message(); + ASSERT_TRUE(loaned_msg.is_valid()); + loaned_msg.get().float64_value = 42.0f; + ASSERT_EQ(42.0f, loaned_msg.get().float64_value); + + SUCCEED(); +} + +TEST_F(TestLoanedMessage, release) { + auto node = std::make_shared("loaned_message_test_node"); + auto pub = node->create_publisher("loaned_message_test_topic", 1); + + MessageT * msg = nullptr; + { + auto loaned_msg = pub->borrow_loaned_message(); + ASSERT_TRUE(loaned_msg.is_valid()); + loaned_msg.get().float64_value = 42.0f; + ASSERT_EQ(42.0f, loaned_msg.get().float64_value); + msg = loaned_msg.release(); + // call destructor implicitly. + // destructor not allowed to free memory because of not having ownership + // of the data after a call to release. + } + + ASSERT_EQ(42.0f, msg->float64_value); + + SUCCEED(); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_logger.cpp b/rclcpp-eloquent/rclcpp/test/test_logger.cpp new file mode 100644 index 0000000..258cdd0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_logger.cpp @@ -0,0 +1,35 @@ +// Copyright 2017 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 + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +TEST(TestLogger, factory_functions) { + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + EXPECT_STREQ("test_logger", logger.get_name()); + rclcpp::Logger logger_copy = rclcpp::Logger(logger); + EXPECT_STREQ("test_logger", logger_copy.get_name()); +} + +TEST(TestLogger, hierarchy) { + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + rclcpp::Logger sublogger = logger.get_child("child"); + EXPECT_STREQ("test_logger.child", sublogger.get_name()); + rclcpp::Logger subsublogger = sublogger.get_child("grandchild"); + EXPECT_STREQ("test_logger.child.grandchild", subsublogger.get_name()); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_logging.cpp b/rclcpp-eloquent/rclcpp/test/test_logging.cpp new file mode 100644 index 0000000..cc8bd62 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_logging.cpp @@ -0,0 +1,275 @@ +// Copyright 2017 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 +#include +#include +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rcutils/logging.h" +#include "rcutils/time.h" + +using ::testing::EndsWith; + +size_t g_log_calls = 0; +rclcpp::Logger g_logger = rclcpp::get_logger("name"); + +struct LogEvent +{ + const rcutils_log_location_t * location; + int level; + std::string name; + rcutils_time_point_value_t timestamp; + std::string message; +}; +LogEvent g_last_log_event; + +class TestLoggingMacros : public ::testing::Test +{ +public: + rcutils_logging_output_handler_t previous_output_handler; + void SetUp() + { + g_log_calls = 0; + ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize()); + rcutils_logging_set_default_logger_level(RCUTILS_LOG_SEVERITY_DEBUG); + + auto rcutils_logging_console_output_handler = []( + const rcutils_log_location_t * location, + int level, const char * name, rcutils_time_point_value_t timestamp, + const char * format, va_list * args) -> void + { + g_log_calls += 1; + g_last_log_event.location = location; + g_last_log_event.level = level; + g_last_log_event.name = name ? name : ""; + g_last_log_event.timestamp = timestamp; + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, *args); + g_last_log_event.message = buffer; + }; + + this->previous_output_handler = rcutils_logging_get_output_handler(); + rcutils_logging_set_output_handler(rcutils_logging_console_output_handler); + } + + void TearDown() + { + rcutils_logging_set_output_handler(this->previous_output_handler); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown()); + EXPECT_FALSE(g_rcutils_logging_initialized); + } +}; + +class DummyNode +{ +public: + DummyNode() + { + clock_ = rclcpp::Clock::make_shared(RCL_ROS_TIME); + } + rclcpp::Clock::SharedPtr get_clock() + { + return clock_; + } + +private: + rclcpp::Clock::SharedPtr clock_; +}; + +TEST_F(TestLoggingMacros, test_logging_named) { + for (int i : {1, 2, 3}) { + RCLCPP_DEBUG(g_logger, "message %d", i); + } + size_t expected_location = __LINE__ - 2u; + EXPECT_EQ(3u, g_log_calls); + EXPECT_TRUE(g_last_log_event.location != NULL); + if (g_last_log_event.location) { + EXPECT_STREQ("TestBody", g_last_log_event.location->function_name); + EXPECT_THAT(g_last_log_event.location->file_name, EndsWith("test_logging.cpp")); + EXPECT_EQ(expected_location, g_last_log_event.location->line_number); + } + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, g_last_log_event.level); + EXPECT_EQ("name", g_last_log_event.name); + EXPECT_EQ("message 3", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_string) { + for (std::string i : {"one", "two", "three"}) { + RCLCPP_DEBUG(g_logger, "message " + i); + } + EXPECT_EQ(3u, g_log_calls); + EXPECT_EQ("message three", g_last_log_event.message); + + RCLCPP_DEBUG(g_logger, "message " "four"); + EXPECT_EQ("message four", g_last_log_event.message); + + RCLCPP_DEBUG(g_logger, std::string("message " "five")); + EXPECT_EQ("message five", g_last_log_event.message); + + RCLCPP_DEBUG(g_logger, std::string("message %s"), "six"); + EXPECT_EQ("message six", g_last_log_event.message); + + RCLCPP_DEBUG(g_logger, "message seven"); + EXPECT_EQ("message seven", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_stream) { + for (std::string i : {"one", "two", "three"}) { + RCLCPP_DEBUG_STREAM(g_logger, "message " << i); + } + EXPECT_EQ(3u, g_log_calls); + EXPECT_EQ("message three", g_last_log_event.message); + + RCLCPP_DEBUG_STREAM(g_logger, 4 << "th message"); + EXPECT_EQ("4th message", g_last_log_event.message); + + RCLCPP_DEBUG_STREAM(g_logger, "message " << 5); + EXPECT_EQ("message 5", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_once) { + for (int i : {1, 2, 3}) { + RCLCPP_INFO_ONCE(g_logger, "message %d", i); + } + EXPECT_EQ(1u, g_log_calls); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, g_last_log_event.level); + EXPECT_EQ("name", g_last_log_event.name); + EXPECT_EQ("message 1", g_last_log_event.message); + + // Check that another instance has a context that's independent to the call above's + g_log_calls = 0; + for (int i : {1, 2, 3}) { + RCLCPP_INFO_ONCE(g_logger, "second message %d", i); + } + EXPECT_EQ(1u, g_log_calls); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, g_last_log_event.level); + EXPECT_EQ("name", g_last_log_event.name); + EXPECT_EQ("second message 1", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_expression) { + for (int i : {1, 2, 3, 4, 5, 6}) { + RCLCPP_INFO_EXPRESSION(g_logger, i % 3, "message %d", i); + } + EXPECT_EQ(4u, g_log_calls); + EXPECT_EQ("message 5", g_last_log_event.message); +} + +int g_counter = 0; + +bool mod3() +{ + return (g_counter % 3) != 0; +} + +TEST_F(TestLoggingMacros, test_logging_function) { + for (int i : {1, 2, 3, 4, 5, 6}) { + g_counter = i; + RCLCPP_INFO_FUNCTION(g_logger, &mod3, "message %d", i); + } + EXPECT_EQ(4u, g_log_calls); + EXPECT_EQ("message 5", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_skipfirst) { + for (uint32_t i : {1, 2, 3, 4, 5}) { + RCLCPP_WARN_SKIPFIRST(g_logger, "message %u", i); + EXPECT_EQ(i - 1, g_log_calls); + } +} + +TEST_F(TestLoggingMacros, test_throttle) { + using namespace std::chrono_literals; + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + for (uint64_t i = 0; i < 3; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, steady_clock, 10000, "Throttling"); + } + EXPECT_EQ(1u, g_log_calls); + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, steady_clock, 1, "Skip first throttling"); + EXPECT_EQ(1u, g_log_calls); + for (uint64_t i = 0; i < 6; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, steady_clock, 10, "Throttling"); + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, steady_clock, 40, "Throttling"); + std::this_thread::sleep_for(5ms); + } + EXPECT_EQ(4u, g_log_calls); + rclcpp::Clock ros_clock(RCL_ROS_TIME); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock.get_clock_handle())); + RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10000, "Throttling"); + rcl_clock_t * clock = ros_clock.get_clock_handle(); + ASSERT_TRUE(clock); + EXPECT_EQ(4u, g_log_calls); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, RCUTILS_MS_TO_NS(10))); + for (uint64_t i = 0; i < 2; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10, "Throttling"); + if (i == 0) { + EXPECT_EQ(5u, g_log_calls); + rcl_time_point_value_t clock_ns = ros_clock.now().nanoseconds() + RCUTILS_MS_TO_NS(10); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, clock_ns)); + } else { + EXPECT_EQ(6u, g_log_calls); + } + } + DummyNode node; + rcl_clock_t * node_clock = node.get_clock()->get_clock_handle(); + ASSERT_TRUE(node_clock); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(node_clock)); + EXPECT_EQ(6u, g_log_calls); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, RCUTILS_MS_TO_NS(10))); + for (uint64_t i = 0; i < 3; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, *node.get_clock(), 10, "Throttling"); + if (i == 0) { + EXPECT_EQ(7u, g_log_calls); + rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns)); + } else if (i == 1) { + EXPECT_EQ(7u, g_log_calls); + rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns)); + } else { + EXPECT_EQ(8u, g_log_calls); + } + } +} + +bool log_function(rclcpp::Logger logger) +{ + RCLCPP_INFO(logger, "successful log"); + return true; +} + +bool log_function_const(const rclcpp::Logger logger) +{ + RCLCPP_INFO(logger, "successful log"); + return true; +} + +bool log_function_const_ref(const rclcpp::Logger & logger) +{ + RCLCPP_INFO(logger, "successful log"); + return true; +} + +TEST_F(TestLoggingMacros, test_log_from_node) { + auto logger = rclcpp::get_logger("test_logging_logger"); + EXPECT_TRUE(log_function(logger)); + EXPECT_TRUE(log_function_const(logger)); + EXPECT_TRUE(log_function_const_ref(logger)); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_node.cpp b/rclcpp-eloquent/rclcpp/test/test_node.cpp new file mode 100644 index 0000000..1eddeea --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_node.cpp @@ -0,0 +1,2508 @@ +// Copyright 2017 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 +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/scope_exit.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/filesystem_helper.hpp" + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() override + { + test_resources_path /= "test_node"; + } + + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; +}; + +/* + Testing node construction and destruction. + */ +TEST_F(TestNode, construction_and_destruction) { + { + std::make_shared("my_node", "/ns"); + } + + { + ASSERT_THROW( + { + std::make_shared("invalid_node?", "/ns"); + }, rclcpp::exceptions::InvalidNodeNameError); + } + + { + ASSERT_THROW( + { + std::make_shared("my_node", "/invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } +} + +TEST_F(TestNode, get_name_and_namespace) { + { + auto node = std::make_shared("my_node", "/ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/ns", node->get_namespace()); + EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name()); + } + { + auto options = rclcpp::NodeOptions() + .arguments({"--ros-args", "-r", "__ns:=/another_ns"}); + auto node = std::make_shared("my_node", "/ns", options); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/another_ns", node->get_namespace()); + EXPECT_STREQ("/another_ns/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", "ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/ns", node->get_namespace()); + EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/", node->get_namespace()); + EXPECT_STREQ("/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", ""); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/", node->get_namespace()); + EXPECT_STREQ("/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", "/my/ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/my/ns", node->get_namespace()); + EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", "my/ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/my/ns", node->get_namespace()); + EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name()); + } + { + auto node1 = std::make_shared("my_node1", "my/ns"); + auto node2 = std::make_shared("my_node2", "my/ns"); + auto node3 = std::make_shared("my_node3", "/ns2"); + auto node4 = std::make_shared("my_node4", "my/ns3"); + auto names_and_namespaces = node1->get_node_names(); + auto name_namespace_set = std::unordered_set( + names_and_namespaces.begin(), + names_and_namespaces.end()); + std::function Set_Contains = [&](std::string string_key) + { + return name_namespace_set.find(string_key) != name_namespace_set.end(); + }; + EXPECT_TRUE(Set_Contains("/my/ns/my_node1")); + EXPECT_TRUE(Set_Contains("/my/ns/my_node2")); + EXPECT_TRUE(Set_Contains("/ns2/my_node3")); + EXPECT_TRUE(Set_Contains("/my/ns3/my_node4")); + } +} + +TEST_F(TestNode, subnode_get_name_and_namespace) { + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("sub_ns"); + EXPECT_STREQ("my_node", subnode->get_name()); + EXPECT_STREQ("/ns", subnode->get_namespace()); + EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str()); + EXPECT_STREQ("/ns/sub_ns", subnode->get_effective_namespace().c_str()); + } + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("sub_ns"); + EXPECT_STREQ("my_node", subnode->get_name()); + EXPECT_STREQ("/ns", subnode->get_namespace()); + EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str()); + EXPECT_STREQ("/ns/sub_ns", subnode->get_effective_namespace().c_str()); + auto subnode2 = subnode->create_sub_node("sub_ns2"); + EXPECT_STREQ("my_node", subnode2->get_name()); + EXPECT_STREQ("/ns", subnode2->get_namespace()); + EXPECT_STREQ("sub_ns/sub_ns2", subnode2->get_sub_namespace().c_str()); + EXPECT_STREQ("/ns/sub_ns/sub_ns2", subnode2->get_effective_namespace().c_str()); + } + { + auto node = std::make_shared("my_node"); + auto subnode = node->create_sub_node("sub_ns"); + EXPECT_STREQ("my_node", subnode->get_name()); + EXPECT_STREQ("/", subnode->get_namespace()); + EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str()); + EXPECT_STREQ("/sub_ns", subnode->get_effective_namespace().c_str()); + auto subnode2 = subnode->create_sub_node("sub_ns2"); + EXPECT_STREQ("my_node", subnode2->get_name()); + EXPECT_STREQ("/", subnode2->get_namespace()); + EXPECT_STREQ("sub_ns/sub_ns2", subnode2->get_sub_namespace().c_str()); + EXPECT_STREQ("/sub_ns/sub_ns2", subnode2->get_effective_namespace().c_str()); + } + { + auto node = std::make_shared("my_node"); + ASSERT_THROW( + { + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::NameValidationError); + } +} +/* + Testing node construction and destruction. + */ +TEST_F(TestNode, subnode_construction_and_destruction) { + { + ASSERT_NO_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("sub_ns"); + }); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns/"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns/"); + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::NameValidationError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("~sub_ns"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_NO_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("sub_ns"); + }); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::NameValidationError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("~sub_ns"); + }, rclcpp::exceptions::InvalidNamespaceError); + } +} + +TEST_F(TestNode, get_logger) { + { + auto node = std::make_shared("my_node"); + EXPECT_STREQ("my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "/ns"); + EXPECT_STREQ("ns.my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "ns"); + EXPECT_STREQ("ns.my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "/my/ns"); + EXPECT_STREQ("my.ns.my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "my/ns"); + EXPECT_STREQ("my.ns.my_node", node->get_logger().get_name()); + } +} + +TEST_F(TestNode, get_clock) { + auto node = std::make_shared("my_node", "/ns"); + auto ros_clock = node->get_clock(); + EXPECT_TRUE(ros_clock != nullptr); + EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME); +} + +TEST_F(TestNode, now) { + auto node = std::make_shared("my_node", "/ns"); + auto clock = node->get_clock(); + auto now_builtin = node->now().nanoseconds(); + auto now_external = clock->now().nanoseconds(); + EXPECT_GE(now_external, now_builtin); + EXPECT_LT(now_external - now_builtin, 5000000L); +} + +std::string +operator"" _unq(const char * prefix, size_t prefix_length) +{ + static uint64_t count = 0; + return std::string(prefix, prefix_length) + "_" + std::to_string(++count); +} + +TEST_F(TestNode, declare_parameter_with_no_initial_values) { + // test cases without initial values + auto node = std::make_shared("test_declare_parameter_node"_unq); + { + // no default, no initial + rclcpp::ParameterValue value = node->declare_parameter("parameter"_unq); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + } + { + // int default, no initial + rclcpp::ParameterValue default_value(42); + rclcpp::ParameterValue value = node->declare_parameter("parameter"_unq, default_value); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), default_value.get()); + } + { + // int default, no initial, custom parameter descriptor + auto name = "parameter"_unq; + rclcpp::ParameterValue default_value(42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + rclcpp::ParameterValue value = + node->declare_parameter(name, default_value, descriptor); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), default_value.get()); + rcl_interfaces::msg::ParameterDescriptor actual_descriptor = + node->describe_parameter(name); + EXPECT_EQ(actual_descriptor.read_only, descriptor.read_only); + } + { + // int default, no initial, implicit template specialization + int default_value = 42; + EXPECT_EQ(node->declare_parameter("parameter"_unq, default_value), default_value); + } + { + // parameter already declared throws + auto name = "parameter"_unq; + node->declare_parameter(name); + EXPECT_THROW( + {node->declare_parameter(name);}, + rclcpp::exceptions::ParameterAlreadyDeclaredException); + } + { + // parameter name invalid throws + EXPECT_THROW( + {node->declare_parameter("");}, + rclcpp::exceptions::InvalidParametersException); + } + { + // parameter rejected throws + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto name = "parameter"_unq; + auto on_set_parameters = + [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == name && + parameter.get_type() != rclcpp::PARAMETER_INTEGER) + { + result.successful = false; + result.reason = "'" + name + "' must be an integer"; + } + } + return result; + }; + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + EXPECT_THROW( + {node->declare_parameter(name, "not an int");}, + rclcpp::exceptions::InvalidParameterValueException); + } +} + +auto get_fixed_on_parameter_set_callback(const std::string & name, bool successful) +{ + return + [name, successful](const std::vector & parameters) { + (void)parameters; + rcl_interfaces::msg::SetParametersResult result; + result.successful = successful; + return result; + }; +} + +TEST_F(TestNode, test_registering_multiple_callbacks_api) { + auto node = std::make_shared("test_declare_parameter_node"_unq); + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + auto scoped_callback1 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, true)); + EXPECT_NE(scoped_callback1, nullptr); + int64_t value{node->declare_parameter(name1, default_value)}; + EXPECT_EQ(value, default_value); + + auto name2 = "parameter"_unq; + auto scoped_callback2 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name2, false)); + EXPECT_NE(scoped_callback2, nullptr); + EXPECT_THROW( + {node->declare_parameter(name2, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + auto name3 = "parameter"_unq; + scoped_callback2.reset(); + value = node->declare_parameter(name3, default_value); + EXPECT_EQ(value, default_value); + } + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + auto scoped_callback1 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, true)); + EXPECT_NE(scoped_callback1, nullptr); + int64_t value{node->declare_parameter(name1, default_value)}; + EXPECT_EQ(value, default_value); + + auto name2 = "parameter"_unq; + auto scoped_callback2 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name2, false)); + EXPECT_NE(scoped_callback2, nullptr); + EXPECT_THROW( + {node->declare_parameter(name2, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + auto name3 = "parameter"_unq; + node->remove_on_set_parameters_callback(scoped_callback2.get()); + value = node->declare_parameter(name3, default_value); + EXPECT_EQ(value, default_value); + } + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback( + node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, false))); + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback_copy(scoped_callback); + scoped_callback.reset(); + + EXPECT_THROW( + {node->declare_parameter("parameter"_unq, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + scoped_callback_copy.reset(); + // All the shared_ptr has been reset + int64_t value = node->declare_parameter("parameter"_unq, default_value); + EXPECT_EQ(value, default_value); + } +} + +TEST_F(TestNode, declare_parameter_with_overrides) { + // test cases with overrides + rclcpp::NodeOptions no; + no.parameter_overrides( + { + {"parameter_no_default", 42}, + {"parameter_no_default_set", 42}, + {"parameter_no_default_set_cvref", 42}, + {"parameter_and_default", 42}, + {"parameter_and_default_ignore_override", 42}, + {"parameter_custom", 42}, + {"parameter_template", 42}, + {"parameter_already_declared", 42}, + {"parameter_rejected", 42}, + {"parameter_type_mismatch", "not an int"}, + }); + + auto node = std::make_shared("test_declare_parameter_node"_unq, no); + { + // no default, with override + rclcpp::ParameterValue value = node->declare_parameter("parameter_no_default"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); + } + { + // no default, with override, and set after + rclcpp::ParameterValue value = node->declare_parameter("parameter_no_default_set"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); + // check that the value is changed after a set + node->set_parameter({"parameter_no_default_set", 44}); + EXPECT_EQ(node->get_parameter("parameter_no_default_set").get_value(), 44); + } + { + // no default, with override + const rclcpp::ParameterValue & value = + node->declare_parameter("parameter_no_default_set_cvref"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); + // check that the value is changed after a set + node->set_parameter({"parameter_no_default_set_cvref", 44}); + EXPECT_EQ(value.get(), 44); + } + { + // int default, with override + rclcpp::ParameterValue default_value(43); + rclcpp::ParameterValue value = node->declare_parameter("parameter_and_default", default_value); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); // and not 43 which is the default value + } + { + // int default, with override and ignoring it + rclcpp::ParameterValue default_value(43); + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_and_default_ignore_override", + default_value, + rcl_interfaces::msg::ParameterDescriptor(), + true); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 43); // and not 42, the parameter override is ignored. + } + { + // int default, with initial, custom parameter descriptor + rclcpp::ParameterValue default_value(43); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + rclcpp::ParameterValue value = + node->declare_parameter("parameter_custom", default_value, descriptor); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); // and not 43 which is the default value + rcl_interfaces::msg::ParameterDescriptor actual_descriptor = + node->describe_parameter("parameter_custom"); + EXPECT_EQ(actual_descriptor.read_only, descriptor.read_only); + } + { + // int default, with initial, implicit template specialization + int default_value = 43; + // is equal to 42, not 43 which is the default value + EXPECT_EQ(node->declare_parameter("parameter_template", default_value), 42); + } + { + // parameter already declared throws + auto name = "parameter_already_declared"; + node->declare_parameter(name); + EXPECT_THROW( + {node->declare_parameter(name);}, + rclcpp::exceptions::ParameterAlreadyDeclaredException); + } + { + // parameter name invalid throws + EXPECT_THROW( + {node->declare_parameter("");}, + rclcpp::exceptions::InvalidParametersException); + } + { + // parameter rejected throws, with initial value + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto name = std::string("parameter_rejected"); + auto on_set_parameters = + [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == name && + parameter.get_type() == rclcpp::PARAMETER_INTEGER) + { + if (parameter.get_value() < 43) { + result.successful = false; + result.reason = "'" + name + "' must be an integer and less than 43"; + } + } + } + return result; + }; + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + EXPECT_THROW( + {node->declare_parameter(name, 43);}, + rclcpp::exceptions::InvalidParameterValueException); + } + { + // default type and initial value type do not match + EXPECT_THROW( + {node->declare_parameter("parameter_type_mismatch", 42);}, + rclcpp::ParameterTypeException); + } +} + +TEST_F(TestNode, declare_parameters_with_no_initial_values) { + // test cases without initial values + auto node = std::make_shared("test_declare_parameters_node"_unq); + { + // with namespace, defaults, no custom descriptors, no initial + int64_t bigger_than_int = INT64_MAX - 42; + auto values = node->declare_parameters( + "namespace1", { + {"parameter_a", 42}, + {"parameter_b", 256}, + {"parameter_c", bigger_than_int}, + }); + std::vector expected = {42, 256, bigger_than_int}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("namespace1.parameter_a")); + EXPECT_TRUE(node->has_parameter("namespace1.parameter_b")); + EXPECT_FALSE(node->has_parameter("namespace1")); + } + { + // without namespace, defaults, no custom descriptors, no initial + auto values = node->declare_parameters( + "", { + {"parameter_without_ns_a", 42}, + {"parameter_without_ns_b", 256}, + }); + std::vector expected = {42, 256}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_a")); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_b")); + } + { + // with namespace, defaults, custom descriptors, no initial + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + auto values = node->declare_parameters( + "namespace2", { + {"parameter_a", {42, descriptor}}, + {"parameter_b", {256, descriptor}}, + }); + std::vector expected = {42, 256}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("namespace2.parameter_a")); + EXPECT_TRUE(node->has_parameter("namespace2.parameter_b")); + EXPECT_FALSE(node->has_parameter("namespace2")); + } + { + // without namespace, defaults, custom descriptors, no initial + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + auto values = node->declare_parameters( + "", { + {"parameter_without_ns_c", {42, descriptor}}, + {"parameter_without_ns_d", {256, descriptor}}, + }); + std::vector expected = {42, 256}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_c")); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_d")); + } + { + // empty parameters + auto values = node->declare_parameters("", {}); + std::vector expected {}; + EXPECT_EQ(values, expected); + } + { + // parameter already declared throws, even with not_set type + auto name = "parameter"_unq; + node->declare_parameter(name); + EXPECT_THROW( + {node->declare_parameters("", {{name, 42}});}, + rclcpp::exceptions::ParameterAlreadyDeclaredException); + } + { + // parameter name invalid throws + EXPECT_THROW( + {node->declare_parameters("", {{"", 42}});}, + rclcpp::exceptions::InvalidParametersException); + } + { + // parameter rejected throws + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto name = "parameter"_unq; + auto on_set_parameters = + [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == name && + parameter.get_type() != rclcpp::PARAMETER_INTEGER) + { + result.successful = false; + result.reason = "'" + name + "' must be an integer"; + } + } + return result; + }; + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + EXPECT_THROW( + {node->declare_parameters("", {{name, "not an int"}});}, + rclcpp::exceptions::InvalidParameterValueException); + } +} + +TEST_F(TestNode, declare_parameter_with_cli_overrides) { + const std::string parameters_filepath = ( + test_resources_path / "test_parameters.yaml").string(); + // test cases with overrides + rclcpp::NodeOptions no; + no.arguments({ + "--ros-args", + "-p", "parameter_bool:=true", + "-p", "parameter_int:=42", + "-p", "parameter_double:=0.42", + "-p", "parameter_string:=foo", + "--params-file", parameters_filepath.c_str(), + "-p", "parameter_bool_array:=[false, true]", + "-p", "parameter_int_array:=[-21, 42]", + "-p", "parameter_double_array:=[-1.0, .42]", + "-p", "parameter_string_array:=[foo, bar]" + }); + + auto node = std::make_shared("test_declare_parameter_node"_unq, no); + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_bool"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL); + EXPECT_EQ(value.get(), true); + } + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_int"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 21); // set to 42 in CLI, overriden by file + } + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_double"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get(), 0.42); + } + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_string"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); + EXPECT_EQ(value.get(), "foo"); + } + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_bool_array"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL_ARRAY); + std::vector expected_value{false, true}; + EXPECT_EQ(value.get>(), expected_value); + } + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_int_array"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER_ARRAY); + std::vector expected_value{-21, 42}; + EXPECT_EQ(value.get>(), expected_value); + } + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_double_array"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE_ARRAY); + std::vector expected_value{-1.0, 0.42}; + EXPECT_EQ(value.get>(), expected_value); + } + { + rclcpp::ParameterValue value = node->declare_parameter("parameter_string_array"); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING_ARRAY); + std::vector expected_value{"foo", "bar"}; + // set to [baz, baz, baz] in file, overriden by CLI + EXPECT_EQ(value.get>(), expected_value); + } +} + +TEST_F(TestNode, undeclare_parameter) { + auto node = std::make_shared("test_undeclare_parameter_node"_unq); + { + // normal use + auto name = "parameter"_unq; + node->declare_parameter(name); + EXPECT_TRUE(node->has_parameter(name)); + node->undeclare_parameter(name); + EXPECT_FALSE(node->has_parameter(name)); + } + { + // not declared throws + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + EXPECT_THROW( + {node->undeclare_parameter(name);}, + rclcpp::exceptions::ParameterNotDeclaredException); + } + { + // read only parameter throws + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_THROW( + {node->undeclare_parameter(name);}, + rclcpp::exceptions::ParameterImmutableException); + EXPECT_TRUE(node->has_parameter(name)); + } +} + +TEST_F(TestNode, has_parameter) { + auto node = std::make_shared("test_has_parameter_node"_unq); + { + // normal use + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name); + EXPECT_TRUE(node->has_parameter(name)); + node->undeclare_parameter(name); + EXPECT_FALSE(node->has_parameter(name)); + } +} + +TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_set_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } + { + // normal use, change type + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, "not an integer")).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), std::string("not an integer")); + } + { + // normal use, multiple parameters + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + node->declare_parameter(name2, true); + EXPECT_TRUE(node->has_parameter(name2)); + node->declare_parameter(name3, "something"); + EXPECT_TRUE(node->has_parameter(name3)); + + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name1, "not an integer")).successful); + EXPECT_EQ(node->get_parameter(name1).get_value(), std::string("not an integer")); + + EXPECT_EQ(node->get_parameter(name2).get_value(), true); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name2, false)).successful); + EXPECT_EQ(node->get_parameter(name2).get_value(), false); + } + { + // setting an undeclared parameter throws + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + EXPECT_THROW( + {node->set_parameter(rclcpp::Parameter(name, 42));}, + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_FALSE(node->has_parameter(name)); + } + { + // rejecting parameter does not throw, but fails + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [](const std::vector &) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = "no parameter may not be set right now"; + return result; + }; + node->set_on_parameters_set_callback(on_set_parameters); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare + auto name = "parameter"_unq; + auto value = node->declare_parameter(name); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name)).successful); + + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will undeclare + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name)).successful); + + EXPECT_FALSE(node->has_parameter(name)); + } + { + // setting a parameter with integer range descriptor + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 2; + node->declare_parameter(name, 10, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 10); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 14)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 14); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 15)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 20)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 8)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, from_value > to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 20; + integer_range.to_value = 18; + integer_range.step = 1; + node->declare_parameter(name, 20, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 20); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 10)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, from_value = to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 18; + integer_range.step = 1; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, step > distance(from_value, to_value) + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 25; + integer_range.step = 10; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 26)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + } + { + // setting a parameter with integer range descriptor, distance not multiple of the step. + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 28; + integer_range.step = 7; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 28)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 28); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 32)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + } + { + // setting a parameter with integer range descriptor, step=0 + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 0; + node->declare_parameter(name, 10, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 10); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 15)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 15); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor and wrong default value will throw + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 2; + ASSERT_THROW( + node->declare_parameter(name, 42, descriptor), + rclcpp::exceptions::InvalidParameterValueException); + } + { + // setting a parameter with floating point range descriptor + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.2); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, negative step + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = -0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.2); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, from_value > to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 11.0; + floating_point_range.to_value = 10.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, from_value = to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 10.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + } + { + // setting a parameter with floating point range descriptor, step > distance + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 2.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 7.8)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, distance not multiple of the step. + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.7; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.7)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + } + { + // setting a parameter with floating point range descriptor, step=0 + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.0; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.0001)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0001); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.5479051)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.5479051); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.001)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.999)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with a different type is still possible + // when having a descriptor specifying a type (type is a status, not a constraint). + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 42); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, "asd")).successful); + EXPECT_TRUE(node->has_parameter(name)); + value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); + EXPECT_EQ(value.get_value(), "asd"); + } +} + +TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { + rclcpp::NodeOptions no; + no.parameter_overrides( + { + {"parameter_with_override", 30}, + }); + no.allow_undeclared_parameters(true); + auto node = std::make_shared("test_set_parameter_node"_unq, no); + { + // overrides are ignored when not declaring a parameter + auto name = "parameter_with_override"; + EXPECT_FALSE(node->has_parameter(name)); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } + { + // normal use (declare first) still works with this true + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } + { + // setting a parameter that is not declared implicitly declares it + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } +} + +TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_set_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + auto rets = node->set_parameters( + { + {name1, 2}, + {name2, false}, + {name3, "red"}, + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + } + { + // overwrite and order of setting + auto name = "parameter"_unq; + node->declare_parameter(name, 1); + + auto rets = node->set_parameters( + { + {name, 42}, + {name, 2}, + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 2); + } + { + // undeclared parameter throws, + // and preceding parameters are still set, but proceeding values are not + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name3, 100); + + EXPECT_THROW( + { + node->set_parameters( + { + {name1, 2}, + {name2, "not declared :("}, + {name3, 101}, + }); + }, + rclcpp::exceptions::ParameterNotDeclaredException); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 100); + } + { + // rejecting parameter does not throw, but fails + // all parameters to be set are attempted to be set + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&name2](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name2;})) { + result.successful = false; + result.reason = "parameter '" + name2 + "' may not be set right now"; + } + return result; + }; + node->set_on_parameters_set_callback(on_set_parameters); + + auto rets = node->set_parameters( + { + {name1, 2}, + {name2, false}, + {name3, "red"}, + }); + EXPECT_EQ(rets.size(), 3U); + EXPECT_TRUE(rets[0].successful); + EXPECT_FALSE(rets[1].successful); + EXPECT_NE(rets[1].reason.find("may not be set right now"), std::string::npos); + EXPECT_TRUE(rets[2].successful); + EXPECT_EQ(node->get_parameter(name1).get_value(), 2); + EXPECT_EQ(node->get_parameter(name2).get_value(), true); // old value + EXPECT_EQ(node->get_parameter(name3).get_value(), "red"); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare + auto name = "parameter"_unq; + auto value = node->declare_parameter(name); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + + EXPECT_TRUE(node->set_parameters({rclcpp::Parameter(name)})[0].successful); + + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will undeclare + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_TRUE(node->set_parameters({rclcpp::Parameter(name)})[0].successful); + + EXPECT_FALSE(node->has_parameter(name)); + } +} + +// test set_parameters with undeclared allowed +TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_set_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works with this true + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + node->declare_parameter(name2, "test"); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + + auto rets = node->set_parameters( + { + rclcpp::Parameter(name1, 43), + rclcpp::Parameter(name2, "other"), + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_EQ(node->get_parameter(name1).get_value(), 43); + EXPECT_EQ(node->get_parameter(name2).get_value(), "other"); + } + { + // setting a parameter that is not declared implicitly declares it + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + auto rets = node->set_parameters( + { + rclcpp::Parameter(name1, 42), + rclcpp::Parameter(name2, "test"), + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + } +} + +TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_set_parameters_atomically_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + auto ret = node->set_parameters_atomically( + { + {name1, 2}, + {name2, false}, + {name3, "red"}, + }); + EXPECT_TRUE(ret.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + } + { + // overwrite and order of setting + auto name = "parameter"_unq; + node->declare_parameter(name, 1); + + auto ret = node->set_parameters_atomically( + { + {name, 42}, + {name, 2}, + }); + EXPECT_TRUE(ret.successful); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 2); + } + { + // undeclared parameter throws, + // and no parameters were changed + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name3, 100); + + EXPECT_THROW( + { + node->set_parameters_atomically( + { + {name1, 2}, + {name2, "not declared :("}, + {name3, 101}, + }); + }, + rclcpp::exceptions::ParameterNotDeclaredException); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + // both have old values + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name3).get_value(), 100); + } + { + // rejecting parameter does not throw, but fails + // and no parameters are changed + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&name2](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name2;})) { + result.successful = false; + result.reason = "parameter '" + name2 + "' may not be set right now"; + } + return result; + }; + node->set_on_parameters_set_callback(on_set_parameters); + + auto ret = node->set_parameters_atomically( + { + {name1, 2}, + {name2, false}, // should fail to be set, failing the whole operation + {name3, "red"}, + }); + EXPECT_FALSE(ret.successful); + // all have old values + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), true); + EXPECT_EQ(node->get_parameter(name3).get_value(), "blue"); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare + auto name = "parameter"_unq; + auto value = node->declare_parameter(name); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + + EXPECT_TRUE(node->set_parameters_atomically({rclcpp::Parameter(name)}).successful); + + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will undeclare + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_TRUE(node->set_parameters_atomically({rclcpp::Parameter(name)}).successful); + + EXPECT_FALSE(node->has_parameter(name)); + } +} + +// test set_parameters with undeclared allowed +TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_set_parameters_atomically_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works with this true + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + node->declare_parameter(name2, "test"); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + + auto ret = node->set_parameters_atomically( + { + rclcpp::Parameter(name1, 43), + rclcpp::Parameter(name2, "other"), + }); + EXPECT_TRUE(ret.successful); + EXPECT_EQ(node->get_parameter(name1).get_value(), 43); + EXPECT_EQ(node->get_parameter(name2).get_value(), "other"); + } + { + // setting a parameter that is not declared implicitly declares it + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + auto ret = node->set_parameters_atomically( + { + rclcpp::Parameter(name1, 42), + rclcpp::Parameter(name2, "test"), + }); + EXPECT_TRUE(ret.successful); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + } + { + // if an undeclared parameter is implicitly declared, but a later parameter set fails, + // then the implicitly "to be" declared parameter remains undeclared + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + node->declare_parameter(name3, "test"); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name3).get_value(), "test"); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&name3](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name3;})) { + result.successful = false; + result.reason = "parameter '" + name3 + "' may not be set right now"; + } + return result; + }; + node->set_on_parameters_set_callback(on_set_parameters); + + auto ret = node->set_parameters_atomically( + { + rclcpp::Parameter(name1, 43), + rclcpp::Parameter(name2, true), // this would cause implicit declaration + rclcpp::Parameter(name3, "other"), // this set should fail, and fail the whole operation + }); + EXPECT_FALSE(ret.successful); + // name1 and name2 remain with the old values + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + EXPECT_FALSE(node->has_parameter(name2)); // important! name2 remains undeclared + EXPECT_EQ(node->get_parameter(name3).get_value(), "test"); + } +} + +// test get_parameter with undeclared not allowed +TEST_F(TestNode, get_parameter_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + // version that throws on undeclared + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + // version that returns bool and never throws, and stores in rclcpp::Parameter + { + rclcpp::Parameter parameter; + EXPECT_TRUE(node->get_parameter(name, parameter)); + EXPECT_EQ(parameter.get_value(), 42); + } + // version that returns bool and never throws, but is templated to store in a primitive type + { + int value; + EXPECT_TRUE(node->get_parameter(name, value)); + EXPECT_EQ(value, 42); + } + } + { + // getting an undeclared parameter throws + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_THROW({node->get_parameter(name);}, rclcpp::exceptions::ParameterNotDeclaredException); + { + rclcpp::Parameter parameter; + EXPECT_FALSE(node->get_parameter(name, parameter)); + } + { + int value; + EXPECT_FALSE(node->get_parameter(name, value)); + } + } + { + // for templated version, throws if the parameter type doesn't match the requested type + auto name = "parameter"_unq; + + node->declare_parameter(name, "not an int"); + + EXPECT_THROW( + { + int value; + node->get_parameter(name, value); + }, + rclcpp::ParameterTypeException); + } +} + +// test get_parameter with undeclared allowed +TEST_F(TestNode, get_parameter_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + // version that throws on undeclared + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + // version that returns bool and never throws, and stores in rclcpp::Parameter + { + rclcpp::Parameter parameter; + EXPECT_TRUE(node->get_parameter(name, parameter)); + EXPECT_EQ(parameter.get_value(), 42); + } + // version that returns bool and never throws, but is templated to store in a primitive type + { + int value; + EXPECT_TRUE(node->get_parameter(name, value)); + EXPECT_EQ(value, 42); + } + } + { + // getting an undeclared parameter returns default constructed rclcpp::Parameter or false + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_EQ(node->get_parameter(name).get_type(), rclcpp::PARAMETER_NOT_SET); + { + rclcpp::Parameter parameter; + EXPECT_FALSE(node->get_parameter(name, parameter)); + } + { + int value; + EXPECT_FALSE(node->get_parameter(name, value)); + } + } + { + // for templated version, return false if the parameter not declared + auto name = "parameter"_unq; + + EXPECT_EQ(node->get_parameter(name).get_type(), rclcpp::PARAMETER_NOT_SET); + int value = 42; + EXPECT_FALSE(node->get_parameter(name, value)); + EXPECT_EQ(value, 42); + } +} + +// test get_parameter_or with undeclared not allowed +TEST_F(TestNode, get_parameter_or_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_or_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + { + int value; + EXPECT_TRUE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 42); + } + } + { + // normal use, no declare first + auto name = "parameter"_unq; + + { + int value; + EXPECT_FALSE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 43); + } + } +} + +// test get_parameter_or with undeclared allowed +TEST_F(TestNode, get_parameter_or_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + { + int value; + EXPECT_TRUE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 42); + } + } + { + // normal use, no declare first + auto name = "parameter"_unq; + + { + int value; + EXPECT_FALSE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 43); + } + } +} + +// test get_parameters with undeclared not allowed +TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto base_name1 = "parameter"_unq; + auto name1 = "ints." + base_name1; + auto base_name2 = "parameter"_unq; + auto name2 = "strings." + base_name2; + auto base_name3 = "parameter"_unq; + auto name3 = "ints." + base_name3; + + node->declare_parameter(name1, 42); + node->declare_parameter(name2, "test"); + node->declare_parameter(name3, 100); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + + // non-templated version, get all + { + std::vector expected = { + {name1, 42}, + {name2, "test"}, + {name3, 100}, + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3}), expected); + } + // non-templated version, get some + { + std::vector expected = { + {name1, 42}, + {name3, 100}, + }; + EXPECT_EQ(node->get_parameters({name1, name3}), expected); + } + // non-templated version, get some, different types + { + std::vector expected = { + {name1, 42}, + {name2, "test"}, + }; + EXPECT_EQ(node->get_parameters({name1, name2}), expected); + } + // non-templated version, get some, wrong order (request order preserved) + { + std::vector expected = { + {name3, 100}, + {name1, 42}, + }; + EXPECT_EQ(node->get_parameters({name3, name1}), expected); + } + // templated version, get all int's + { + std::map expected = { + {base_name1, 42}, + {base_name3, 100}, + }; + std::map actual; + EXPECT_TRUE(node->get_parameters("ints", actual)); + EXPECT_EQ(actual, expected); + } + // templated version, get the one string + { + std::map expected = { + {base_name2, "test"}, + }; + std::map actual; + EXPECT_TRUE(node->get_parameters("strings", actual)); + EXPECT_EQ(actual, expected); + } + } + { + // getting an undeclared parameter throws, or in the alternative signature returns false + auto name = "prefix.parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_THROW( + {node->get_parameters({name});}, + rclcpp::exceptions::ParameterNotDeclaredException); + { + std::map values; + EXPECT_TRUE(values.empty()); + EXPECT_FALSE(node->get_parameters("prefix", values)); + EXPECT_TRUE(values.empty()); + } + } + { + // templated version with empty prefix will get all parameters + auto node_local = std::make_shared("test_get_parameters_node"_unq); + auto name1 = "prefix1.parameter"_unq; + auto name2 = "prefix2.parameter"_unq; + + node_local->declare_parameter(name1, 42); + node_local->declare_parameter(name2, 100); + // undeclare so that it doesn't interfere with the test + node_local->undeclare_parameter("use_sim_time"); + + { + std::map actual; + EXPECT_TRUE(node_local->get_parameters("", actual)); + EXPECT_NE(actual.find(name1), actual.end()); + EXPECT_NE(actual.find(name2), actual.end()); + } + + // will throw if set of parameters is non-homogeneous + auto name3 = "prefix2.parameter"_unq; + node_local->declare_parameter(name3, "not an int"); + + { + std::map actual; + EXPECT_THROW( + { + node_local->get_parameters("", actual); + }, + rclcpp::ParameterTypeException); + } + } +} + +// test get_parameters with undeclared allowed +TEST_F(TestNode, get_parameters_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use + auto base_name1 = "parameter"_unq; + auto name1 = "ints." + base_name1; + auto base_name2 = "parameter"_unq; + auto name2 = "strings." + base_name2; + auto base_name3 = "parameter"_unq; + auto name3 = "ints." + base_name3; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + { + // non-templated version, get all, none set, no throw + std::vector expected = { + {name1, {}}, + {name2, {}}, + {name3, {}}, + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3}), expected); + } + { + // templated version, get all int's, none set, no throw + std::map actual; + EXPECT_FALSE(node->get_parameters("ints", actual)); + EXPECT_TRUE(actual.empty()); + } + { + // templated version, get the one string, none set, no throw + std::map actual; + EXPECT_FALSE(node->get_parameters("strings", actual)); + EXPECT_TRUE(actual.empty()); + } + } +} + +// test describe parameter with undeclared not allowed +TEST_F(TestNode, describe_parameter_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + { + auto result = node->describe_parameter(name1); + EXPECT_EQ(result.name, name1); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(result.read_only); + } + { + auto result = node->describe_parameter(name2); + EXPECT_EQ(result.name, name2); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(result.read_only); + } + } + { + // non-existent parameter throws + auto name = "parameter"_unq; + + { + EXPECT_THROW( + { + node->describe_parameter(name); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } +} + +// test describe parameter with undeclared allowed +TEST_F(TestNode, describe_parameter_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use still works + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + { + auto result = node->describe_parameter(name1); + EXPECT_EQ(result.name, name1); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(result.read_only); + } + { + auto result = node->describe_parameter(name2); + EXPECT_EQ(result.name, name2); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(result.read_only); + } + } + { + // non-existent parameter does not throw, but returns default constructed one + auto name = "parameter"_unq; + + { + auto result = node->describe_parameter(name); + rcl_interfaces::msg::ParameterDescriptor expected; + expected.name = name; + EXPECT_EQ(result, expected); + } + } +} + +// test describe parameters with undeclared not allowed +TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->describe_parameters({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0].name, name1); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(results[0].read_only); + + EXPECT_EQ(results[1].name, name2); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[1].read_only); + } + { + // non-existent parameter throws + auto name = "parameter"_unq; + + { + EXPECT_THROW( + { + node->describe_parameters({name}); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } + { + // non-existent parameter throws, even with existing parameters in the list requested + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + + { + EXPECT_THROW( + { + node->describe_parameters({name1, name2}); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->describe_parameters({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0].name, name2); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[0].read_only); + + EXPECT_EQ(results[1].name, name1); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(results[1].read_only); + + EXPECT_EQ(results[2].name, name2); + EXPECT_EQ(results[2].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[2].read_only); + } +} + +// test describe parameters with undeclared allowed +TEST_F(TestNode, describe_parameters_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use still works (declare first) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->describe_parameters({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0].name, name1); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(results[0].read_only); + + EXPECT_EQ(results[1].name, name2); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[1].read_only); + } + { + // non-existent parameter does not throw + auto name = "parameter"_unq; + + auto results = node->describe_parameters({name}); + + EXPECT_EQ(results.size(), 1u); + + EXPECT_EQ(results[0].name, name); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[0].read_only); + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + auto results = node->describe_parameters({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0].name, name2); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[0].read_only); + + EXPECT_EQ(results[1].name, name1); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[1].read_only); + + EXPECT_EQ(results[2].name, name2); + EXPECT_EQ(results[2].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[2].read_only); + } +} + +// test get parameter types with undeclared not allowed +TEST_F(TestNode, get_parameter_types_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_types_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->get_parameter_types({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } + { + // non-existent parameter throws + auto name = "parameter"_unq; + + { + EXPECT_THROW( + { + node->get_parameter_types({name}); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->get_parameter_types({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } +} + +// test get parameter types with undeclared allowed +TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_types_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use still works (declare first) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->get_parameter_types({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } + { + // non-existent parameter does not throw + auto name = "parameter"_unq; + + auto results = node->get_parameter_types({name}); + + EXPECT_EQ(results.size(), 1u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + auto results = node->get_parameter_types({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } +} + +// test that it is possible to call get_parameter within the set_callback +TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) { + auto node = std::make_shared("test_set_callback_get_parameter_node"_unq); + + int64_t intval = node->declare_parameter("intparam", 42); + EXPECT_EQ(intval, 42); + double floatval = node->declare_parameter("floatparam", 5.4); + EXPECT_EQ(floatval, 5.4); + + double floatout; + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&node, &floatout](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 40) { + result.successful = false; + } + + rclcpp::Parameter floatparam = node->get_parameter("floatparam"); + if (floatparam.get_value() != 5.4) { + result.successful = false; + } + floatout = floatparam.get_value(); + + return result; + }; + + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + ASSERT_NO_THROW(node->set_parameter({"intparam", 40})); + ASSERT_EQ(floatout, 5.4); +} + +// test that calling set_parameter inside of a set_callback throws an exception +TEST_F(TestNode, set_on_parameters_set_callback_set_parameter) { + auto node = std::make_shared("test_set_callback_set_parameter_node"_unq); + + int64_t intval = node->declare_parameter("intparam", 42); + EXPECT_EQ(intval, 42); + double floatval = node->declare_parameter("floatparam", 5.4); + EXPECT_EQ(floatval, 5.4); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 40) { + result.successful = false; + } + + // This should throw an exception + node->set_parameter({"floatparam", 5.6}); + + return result; + }; + + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + EXPECT_THROW( + { + node->set_parameter(rclcpp::Parameter("intparam", 40)); + }, rclcpp::exceptions::ParameterModifiedInCallbackException); +} + +// test that calling declare_parameter inside of a set_callback throws an exception +TEST_F(TestNode, set_on_parameters_set_callback_declare_parameter) { + auto node = std::make_shared("test_set_callback_declare_parameter_node"_unq); + + int64_t intval = node->declare_parameter("intparam", 42); + EXPECT_EQ(intval, 42); + double floatval = node->declare_parameter("floatparam", 5.4); + EXPECT_EQ(floatval, 5.4); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 40) { + result.successful = false; + } + + // This should throw an exception + node->declare_parameter("floatparam2", 5.6); + + return result; + }; + + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + EXPECT_THROW( + { + node->set_parameter(rclcpp::Parameter("intparam", 40)); + }, rclcpp::exceptions::ParameterModifiedInCallbackException); +} + +// test that calling undeclare_parameter inside a set_callback throws an exception +TEST_F(TestNode, set_on_parameters_set_callback_undeclare_parameter) { + auto node = std::make_shared("test_set_callback_undeclare_parameter_node"_unq); + + int64_t intval = node->declare_parameter("intparam", 42); + EXPECT_EQ(intval, 42); + double floatval = node->declare_parameter("floatparam", 5.4); + EXPECT_EQ(floatval, 5.4); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 40) { + result.successful = false; + } + + // This should throw an exception + node->undeclare_parameter("floatparam"); + + return result; + }; + + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + EXPECT_THROW( + { + node->set_parameter(rclcpp::Parameter("intparam", 40)); + }, rclcpp::exceptions::ParameterModifiedInCallbackException); +} + +// test that calling set_on_parameters_set_callback from a set_callback throws an exception +TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) { + auto node = std::make_shared("test_set_callback_set_callback_node"_unq); + + int64_t intval = node->declare_parameter("intparam", 42); + EXPECT_EQ(intval, 42); + double floatval = node->declare_parameter("floatparam", 5.4); + EXPECT_EQ(floatval, 5.4); + + RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);}); // always reset + auto on_set_parameters = + [&node](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 40) { + result.successful = false; + } + + auto bad_parameters = + [](const std::vector & parameters) { + (void)parameters; + rcl_interfaces::msg::SetParametersResult result; + return result; + }; + + // This should throw an exception + node->set_on_parameters_set_callback(bad_parameters); + + return result; + }; + + EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr); + EXPECT_THROW( + { + node->set_parameter(rclcpp::Parameter("intparam", 40)); + }, rclcpp::exceptions::ParameterModifiedInCallbackException); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_node_global_args.cpp b/rclcpp-eloquent/rclcpp/test/test_node_global_args.cpp new file mode 100644 index 0000000..febef6b --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_node_global_args.cpp @@ -0,0 +1,60 @@ +// Copyright 2018 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNodeWithGlobalArgs : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + const char * const args[] = {"proc", "--ros-args", "-r", "__node:=global_node_name"}; + const int argc = sizeof(args) / sizeof(const char *); + rclcpp::init(argc, args); + } +}; + +TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) { + auto options = rclcpp::NodeOptions() + .arguments({"--ros-args", "-r", "__node:=local_arguments_test"}); + + auto node = rclcpp::Node::make_shared("orig_name", options); + EXPECT_STREQ("local_arguments_test", node->get_name()); +} + +TEST_F(TestNodeWithGlobalArgs, use_or_ignore_global_arguments) { + { // Don't use global args + auto options = rclcpp::NodeOptions() + .use_global_arguments(false); + + auto node = rclcpp::Node::make_shared("orig_name", options); + EXPECT_STREQ("orig_name", node->get_name()); + } + { // Do use global args + auto options = rclcpp::NodeOptions() + .use_global_arguments(true); + + auto node = rclcpp::Node::make_shared("orig_name", options); + EXPECT_STREQ("global_node_name", node->get_name()); + } +} diff --git a/rclcpp-eloquent/rclcpp/test/test_node_options.cpp b/rclcpp-eloquent/rclcpp/test/test_node_options.cpp new file mode 100644 index 0000000..c4eb969 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_node_options.cpp @@ -0,0 +1,100 @@ +// Copyright 2019 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 +#include + +#include "rcl/allocator.h" +#include "rcl/arguments.h" +#include "rcl/remap.h" + +#include "rclcpp/node_options.hpp" + + +TEST(TestNodeOptions, ros_args_only) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::NodeOptions(allocator) + .arguments({"--ros-args", "-r", "__node:=some_node", "-r", "__ns:=/some_ns"}); + + const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); + ASSERT_TRUE(rcl_options != nullptr); + ASSERT_EQ(0, rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + ASSERT_EQ(0, rcl_arguments_get_count_unparsed_ros(&rcl_options->arguments)); + + char * node_name = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_remap_node_name( + &rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); + ASSERT_TRUE(node_name != nullptr); + EXPECT_STREQ("some_node", node_name); + allocator.deallocate(node_name, allocator.state); + + char * namespace_name = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_remap_node_namespace( + &rcl_options->arguments, nullptr, "my_ns", allocator, &namespace_name)); + ASSERT_TRUE(namespace_name != nullptr); + EXPECT_STREQ("/some_ns", namespace_name); + allocator.deallocate(namespace_name, allocator.state); +} + +TEST(TestNodeOptions, ros_args_and_non_ros_args) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::NodeOptions(allocator).arguments({ + "--non-ros-flag", "--ros-args", "-r", "__node:=some_node", + "-r", "__ns:=/some_ns", "--", "non-ros-arg"}); + + const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); + ASSERT_TRUE(rcl_options != nullptr); + ASSERT_EQ(0, rcl_arguments_get_count_unparsed_ros(&rcl_options->arguments)); + ASSERT_EQ(2, rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + + char * node_name = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_remap_node_name( + &rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); + ASSERT_TRUE(node_name != nullptr); + EXPECT_STREQ("some_node", node_name); + allocator.deallocate(node_name, allocator.state); + + char * namespace_name = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_remap_node_namespace( + &rcl_options->arguments, nullptr, "my_ns", allocator, &namespace_name)); + ASSERT_TRUE(namespace_name != nullptr); + EXPECT_STREQ("/some_ns", namespace_name); + allocator.deallocate(namespace_name, allocator.state); + + int * output_indices = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_arguments_get_unparsed( + &rcl_options->arguments, allocator, &output_indices)); + ASSERT_TRUE(output_indices != nullptr); + const std::vector & args = options.arguments(); + EXPECT_EQ("--non-ros-flag", args[output_indices[0]]); + EXPECT_EQ("non-ros-arg", args[output_indices[1]]); + allocator.deallocate(output_indices, allocator.state); +} + +TEST(TestNodeOptions, bad_ros_args) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::NodeOptions(allocator) + .arguments({"--ros-args", "-r", "foo:="}); + + EXPECT_THROW( + options.get_rcl_node_options(), + rclcpp::exceptions::RCLInvalidROSArgsError); + + options.arguments({"--ros-args", "-r", "foo:=bar", "not-a-ros-arg"}); + EXPECT_THROW( + options.get_rcl_node_options(), + rclcpp::exceptions::UnknownROSArgsError); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_parameter.cpp b/rclcpp-eloquent/rclcpp/test/test_parameter.cpp new file mode 100644 index 0000000..876fbb4 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_parameter.cpp @@ -0,0 +1,769 @@ +// Copyright 2018 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 +#include +#include +#include +#include + +#include "rclcpp/parameter.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestParameter : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST(TestParameter, not_set_variant) { + // Direct instantiation + rclcpp::Parameter not_set_variant; + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type()); + EXPECT_EQ("not set", not_set_variant.get_type_name()); + + EXPECT_THROW(not_set_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_string_array(), rclcpp::ParameterTypeException); + + rcl_interfaces::msg::Parameter not_set_param = not_set_variant.to_parameter_msg(); + EXPECT_EQ("", not_set_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, not_set_param.value.type); + + // From parameter message + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_NOT_SET, + rclcpp::Parameter::from_parameter_msg(not_set_param).get_type()); +} + +TEST(TestParameter, bool_variant) { + // Direct instantiation + rclcpp::Parameter bool_variant_true("bool_param", true); + EXPECT_EQ("bool_param", bool_variant_true.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL, bool_variant_true.get_type()); + EXPECT_EQ("bool", bool_variant_true.get_type_name()); + EXPECT_TRUE(bool_variant_true.get_value()); + EXPECT_TRUE(bool_variant_true.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_true.get_value_message().type); + EXPECT_TRUE(bool_variant_true.as_bool()); + + EXPECT_THROW(bool_variant_true.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("true", bool_variant_true.value_to_string()); + + rclcpp::Parameter bool_variant_false("bool_param", false); + EXPECT_FALSE(bool_variant_false.get_value()); + EXPECT_FALSE(bool_variant_false.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_false.get_value_message().type); + + rcl_interfaces::msg::Parameter bool_param = bool_variant_true.to_parameter_msg(); + EXPECT_EQ("bool_param", bool_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_param.value.type); + EXPECT_TRUE(bool_param.value.bool_value); + + // From parameter message + rclcpp::Parameter from_msg_true = + rclcpp::Parameter::from_parameter_msg(bool_param); + EXPECT_EQ("bool_param", from_msg_true.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL, from_msg_true.get_type()); + EXPECT_EQ("bool", from_msg_true.get_type_name()); + EXPECT_TRUE(from_msg_true.get_value()); + EXPECT_TRUE(from_msg_true.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_false.get_value_message().type); + + bool_param.value.bool_value = false; + rclcpp::Parameter from_msg_false = + rclcpp::Parameter::from_parameter_msg(bool_param); + EXPECT_FALSE(from_msg_false.get_value()); + EXPECT_FALSE(from_msg_false.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_false.get_value_message().type); +} + +TEST(TestParameter, integer_variant) { + const int TEST_VALUE {42}; + + // Direct instantiation + rclcpp::Parameter integer_variant("integer_param", TEST_VALUE); + EXPECT_EQ("integer_param", integer_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, integer_variant.get_type()); + EXPECT_EQ("integer", integer_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + integer_variant.get_value()); + EXPECT_EQ(TEST_VALUE, integer_variant.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + integer_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, integer_variant.as_int()); + + EXPECT_THROW(integer_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("42", integer_variant.value_to_string()); + + rcl_interfaces::msg::Parameter integer_param = integer_variant.to_parameter_msg(); + EXPECT_EQ("integer_param", integer_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, integer_param.value.type); + EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_param); + EXPECT_EQ("integer_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); + EXPECT_EQ("integer", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + from_msg.get_value_message().type); +} + +TEST(TestParameter, long_integer_variant) { + const int64_t TEST_VALUE {std::numeric_limits::max()}; + + // Direct instantiation + rclcpp::Parameter long_variant("long_integer_param", TEST_VALUE); + EXPECT_EQ("long_integer_param", long_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, long_variant.get_type()); + EXPECT_EQ("integer", long_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + long_variant.get_value()); + EXPECT_EQ(TEST_VALUE, long_variant.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + long_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, long_variant.as_int()); + + EXPECT_THROW(long_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("9223372036854775807", long_variant.value_to_string()); + + rcl_interfaces::msg::Parameter integer_param = long_variant.to_parameter_msg(); + EXPECT_EQ("long_integer_param", integer_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, integer_param.value.type); + EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_param); + EXPECT_EQ("long_integer_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); + EXPECT_EQ("integer", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + from_msg.get_value_message().type); +} + +TEST(TestParameter, float_variant) { + const float TEST_VALUE {42.0f}; + + // Direct instantiation + rclcpp::Parameter float_variant("float_param", TEST_VALUE); + EXPECT_EQ("float_param", float_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, float_variant.get_type()); + EXPECT_EQ("double", float_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + float_variant.get_value()); + EXPECT_EQ(TEST_VALUE, float_variant.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + float_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, float_variant.as_double()); + + EXPECT_THROW(float_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("42.000000", float_variant.value_to_string()); + + rcl_interfaces::msg::Parameter float_param = float_variant.to_parameter_msg(); + EXPECT_EQ("float_param", float_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, float_param.value.type); + EXPECT_EQ(TEST_VALUE, float_param.value.double_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(float_param); + EXPECT_EQ("float_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); + EXPECT_EQ("double", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + from_msg.get_value_message().type); +} + +TEST(TestParameter, double_variant) { + const double TEST_VALUE {-42.1}; + + // Direct instantiation + rclcpp::Parameter double_variant("double_param", TEST_VALUE); + EXPECT_EQ("double_param", double_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, double_variant.get_type()); + EXPECT_EQ("double", double_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + double_variant.get_value()); + EXPECT_EQ(TEST_VALUE, double_variant.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + double_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, double_variant.as_double()); + + EXPECT_THROW(double_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("-42.100000", double_variant.value_to_string()); + + rcl_interfaces::msg::Parameter double_param = double_variant.to_parameter_msg(); + EXPECT_EQ("double_param", double_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, double_param.value.type); + EXPECT_EQ(TEST_VALUE, double_param.value.double_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(double_param); + EXPECT_EQ("double_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); + EXPECT_EQ("double", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + from_msg.get_value_message().type); +} + +TEST(TestParameter, string_variant) { + const std::string TEST_VALUE {"ROS2"}; + + // Direct instantiation + rclcpp::Parameter string_variant("string_param", TEST_VALUE); + EXPECT_EQ("string_param", string_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, string_variant.get_type()); + EXPECT_EQ("string", string_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + string_variant.get_value()); + EXPECT_EQ(TEST_VALUE, string_variant.get_value_message().string_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + string_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, string_variant.as_string()); + + EXPECT_THROW(string_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ(TEST_VALUE, string_variant.value_to_string()); + + rcl_interfaces::msg::Parameter string_param = string_variant.to_parameter_msg(); + EXPECT_EQ("string_param", string_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING, string_param.value.type); + EXPECT_EQ(TEST_VALUE, string_param.value.string_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(string_param); + EXPECT_EQ("string_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, from_msg.get_type()); + EXPECT_EQ("string", from_msg.get_type_name()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + from_msg.get_value_message().type); +} + +TEST(TestParameter, byte_array_variant) { + const std::vector TEST_VALUE {0x52, 0x4f, 0x53, 0x32}; + + // Direct instantiation + rclcpp::Parameter byte_array_variant("byte_array_param", TEST_VALUE); + EXPECT_EQ("byte_array_param", byte_array_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_variant.get_type()); + EXPECT_EQ("byte_array", byte_array_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + byte_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, byte_array_variant.get_value_message().byte_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + byte_array_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, byte_array_variant.as_byte_array()); + + EXPECT_THROW(byte_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("[0x52, 0x4f, 0x53, 0x32]", byte_array_variant.value_to_string()); + + rcl_interfaces::msg::Parameter byte_array_param = byte_array_variant.to_parameter_msg(); + EXPECT_EQ("byte_array_param", byte_array_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_param.value.type); + EXPECT_EQ(TEST_VALUE, byte_array_param.value.byte_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(byte_array_param); + EXPECT_EQ("byte_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_type()); + EXPECT_EQ("byte_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().byte_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + from_msg.get_value_message().type); +} + +TEST(TestParameter, bool_array_variant) { + const std::vector TEST_VALUE {false, true, true, false, false, true}; + + // Direct instantiation + rclcpp::Parameter bool_array_variant("bool_array_param", TEST_VALUE); + EXPECT_EQ("bool_array_param", bool_array_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_variant.get_type()); + EXPECT_EQ("bool_array", bool_array_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + bool_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, bool_array_variant.get_value_message().bool_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + bool_array_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, bool_array_variant.as_bool_array()); + + EXPECT_THROW(bool_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("[false, true, true, false, false, true]", bool_array_variant.value_to_string()); + + rcl_interfaces::msg::Parameter bool_array_param = bool_array_variant.to_parameter_msg(); + EXPECT_EQ("bool_array_param", bool_array_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_param.value.type); + EXPECT_EQ(TEST_VALUE, bool_array_param.value.bool_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(bool_array_param); + EXPECT_EQ("bool_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_type()); + EXPECT_EQ("bool_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().bool_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + from_msg.get_value_message().type); +} + +TEST(TestParameter, integer_array_variant) { + const std::vector TEST_VALUE + {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; + + // Direct instantiation + rclcpp::Parameter integer_array_variant("integer_array_param", TEST_VALUE); + + EXPECT_EQ("integer_array_param", integer_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_variant.get_type()); + EXPECT_EQ("integer_array", integer_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_variant.get_value_message().type); + + // No direct comparison of vectors of ints and long ints + const auto & param_value_ref = + integer_array_variant.get_value(); + auto mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value_ref.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value_ref.end(), mismatches.second); + + auto param_value = integer_array_variant.get_value_message().integer_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = integer_array_variant.as_integer_array(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_THROW(integer_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42, -99, 2147483647, -2147483648, 0]", + integer_array_variant.value_to_string()); + + rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg(); + EXPECT_EQ("integer_array_param", integer_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_param.value.type); + + param_value = integer_array_param.value.integer_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_array_param); + EXPECT_EQ("integer_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type()); + EXPECT_EQ("integer_array", from_msg.get_type_name()); + + param_value = from_msg.get_value(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = from_msg.get_value_message().integer_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_EQ( + from_msg.get_value_message().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); +} + +TEST(TestParameter, long_integer_array_variant) { + const std::vector TEST_VALUE + {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; + + rclcpp::Parameter long_array_variant("long_integer_array_param", TEST_VALUE); + EXPECT_EQ("long_integer_array_param", long_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, + long_array_variant.get_type()); + EXPECT_EQ("integer_array", long_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + long_array_variant.get_value_message().type); + EXPECT_EQ( + TEST_VALUE, + long_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, long_array_variant.get_value_message().integer_array_value); + EXPECT_EQ(TEST_VALUE, long_array_variant.as_integer_array()); + + EXPECT_THROW(long_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42, -99, 9223372036854775807, -9223372036854775808, 0]", + long_array_variant.value_to_string()); + + rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg(); + EXPECT_EQ("long_integer_array_param", integer_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_param.value.type); + EXPECT_EQ(TEST_VALUE, integer_array_param.value.integer_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_array_param); + EXPECT_EQ("long_integer_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type()); + EXPECT_EQ("integer_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + from_msg.get_value_message().type); +} + +TEST(TestParameter, float_array_variant) { + const std::vector TEST_VALUE + {42.1f, -99.1f, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1f}; + + // Direct instantiation + rclcpp::Parameter float_array_variant("float_array_param", TEST_VALUE); + + EXPECT_EQ("float_array_param", float_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, + float_array_variant.get_type()); + EXPECT_EQ("double_array", float_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + float_array_variant.get_value_message().type); + + // No direct comparison of vectors of floats and doubles + const auto & param_value_ref = + float_array_variant.get_value(); + auto mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value_ref.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value_ref.end(), mismatches.second); + + auto param_value = float_array_variant.get_value_message().double_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = float_array_variant.as_double_array(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_THROW(float_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]", + float_array_variant.value_to_string()); + + rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg(); + EXPECT_EQ("float_array_param", float_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + float_array_param.value.type); + + param_value = float_array_param.value.double_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(float_array_param); + EXPECT_EQ("float_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type()); + EXPECT_EQ("double_array", from_msg.get_type_name()); + + param_value = from_msg.get_value(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = from_msg.get_value_message().double_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + from_msg.get_value_message().type); +} + +TEST(TestParameter, double_array_variant) { + const std::vector TEST_VALUE + {42.1, -99.1, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1}; + + rclcpp::Parameter double_array_variant("double_array_param", TEST_VALUE); + EXPECT_EQ("double_array_param", double_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_variant.get_type()); + EXPECT_EQ("double_array", double_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_variant.get_value_message().type); + EXPECT_EQ( + TEST_VALUE, + double_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, double_array_variant.get_value_message().double_array_value); + EXPECT_EQ(TEST_VALUE, double_array_variant.as_double_array()); + + EXPECT_THROW(double_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]", + double_array_variant.value_to_string()); + + rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg(); + EXPECT_EQ("double_array_param", double_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_param.value.type); + EXPECT_EQ(TEST_VALUE, double_array_param.value.double_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(double_array_param); + EXPECT_EQ("double_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type()); + EXPECT_EQ("double_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + from_msg.get_value_message().type); +} + +TEST(TestParameter, string_array_variant) { + const std::vector TEST_VALUE {"R", "O", "S2"}; + + // Direct instantiation + rclcpp::Parameter string_array_variant("string_array_param", TEST_VALUE); + EXPECT_EQ("string_array_param", string_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_STRING_ARRAY, + string_array_variant.get_type()); + EXPECT_EQ("string_array", string_array_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + string_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, string_array_variant.get_value_message().string_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + string_array_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, string_array_variant.as_string_array()); + + EXPECT_THROW(string_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_double_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("[R, O, S2]", string_array_variant.value_to_string()); + + rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg(); + EXPECT_EQ("string_array_param", string_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + string_array_param.value.type); + EXPECT_EQ(TEST_VALUE, string_array_param.value.string_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(string_array_param); + EXPECT_EQ("string_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_type()); + EXPECT_EQ("string_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + from_msg.get_value_message().type); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_parameter_client.cpp b/rclcpp-eloquent/rclcpp/test/test_parameter_client.cpp new file mode 100644 index 0000000..2630353 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_parameter_client.cpp @@ -0,0 +1,157 @@ +// Copyright 2019 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 +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/parameter_event.hpp" + +class TestParameterClient : public ::testing::Test +{ +public: + void OnMessage(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) + { + (void)event; + } + +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("test_parameter_client", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +/* + Testing async parameter client construction and destruction. + */ +TEST_F(TestParameterClient, async_construction_and_destruction) { + { + auto asynchronous_client = std::make_shared(node); + (void)asynchronous_client; + } + + { + auto asynchronous_client = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + (void)asynchronous_client; + } + + { + ASSERT_THROW( + { + std::make_shared(node, "invalid_remote_node?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +/* + Testing sync parameter client construction and destruction. + */ +TEST_F(TestParameterClient, sync_construction_and_destruction) { + { + auto synchronous_client = std::make_shared(node); + (void)synchronous_client; + } + + { + auto synchronous_client = std::make_shared( + std::make_shared(), + node); + (void)synchronous_client; + } + + { + auto synchronous_client = std::make_shared( + std::make_shared(), + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + (void)synchronous_client; + } + + { + ASSERT_THROW( + { + std::make_shared(node, "invalid_remote_node?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +/* + Testing different methods for parameter event subscription from asynchronous clients. + */ +TEST_F(TestParameterClient, async_parameter_event_subscription) { + auto callback = std::bind(&TestParameterClient::OnMessage, this, std::placeholders::_1); + { + auto asynchronous_client = std::make_shared(node); + auto event_sub = asynchronous_client->on_parameter_event(callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::AsyncParametersClient::on_parameter_event(node, callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::AsyncParametersClient::on_parameter_event( + node->get_node_topics_interface(), + callback); + (void)event_sub; + } +} + +/* + Testing different methods for parameter event subscription from synchronous clients. + */ +TEST_F(TestParameterClient, sync_parameter_event_subscription) { + auto callback = std::bind(&TestParameterClient::OnMessage, this, std::placeholders::_1); + { + auto synchronous_client = std::make_shared(node); + auto event_sub = synchronous_client->on_parameter_event(callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::SyncParametersClient::on_parameter_event(node, callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::SyncParametersClient::on_parameter_event( + node->get_node_topics_interface(), + callback); + (void)event_sub; + } +} diff --git a/rclcpp-eloquent/rclcpp/test/test_parameter_events_filter.cpp b/rclcpp-eloquent/rclcpp/test/test_parameter_events_filter.cpp new file mode 100644 index 0000000..a497a28 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_parameter_events_filter.cpp @@ -0,0 +1,205 @@ +// Copyright 2017 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 +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/parameter_events_filter.hpp" + +#include "rcl_interfaces/msg/parameter_event.hpp" + +class TestParameterEventFilter : public ::testing::Test +{ +protected: + void SetUp() + { + empty = std::make_shared(); + full = std::make_shared(); + multiple = std::make_shared(); + np = std::make_shared(); + cp = std::make_shared(); + dp = std::make_shared(); + + rcl_interfaces::msg::Parameter p; + p.name = "new"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + p.value.integer_value = 1; + full->new_parameters.push_back(p); + np->new_parameters.push_back(p); + multiple->new_parameters.push_back(p); + + p.name = "new2"; + p.value.integer_value = 2; + multiple->new_parameters.push_back(p); + + p.name = "changed"; + p.value.integer_value = 1; + full->changed_parameters.push_back(p); + cp->changed_parameters.push_back(p); + + p.name = "deleted"; + p.value.integer_value = 1; + full->deleted_parameters.push_back(p); + dp->changed_parameters.push_back(p); + } + + rcl_interfaces::msg::ParameterEvent::SharedPtr empty; + rcl_interfaces::msg::ParameterEvent::SharedPtr full; + rcl_interfaces::msg::ParameterEvent::SharedPtr multiple; + rcl_interfaces::msg::ParameterEvent::SharedPtr np; + rcl_interfaces::msg::ParameterEvent::SharedPtr cp; + rcl_interfaces::msg::ParameterEvent::SharedPtr dp; + + rclcpp::ParameterEventsFilter::EventType nt = rclcpp::ParameterEventsFilter::EventType::NEW; + rclcpp::ParameterEventsFilter::EventType ct = rclcpp::ParameterEventsFilter::EventType::CHANGED; + rclcpp::ParameterEventsFilter::EventType dt = rclcpp::ParameterEventsFilter::EventType::DELETED; +}; + +/* + Testing filters. + */ +TEST_F(TestParameterEventFilter, full_by_type) { + auto res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(3u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, ct}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {ct}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {dt}); + EXPECT_EQ(1u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, full_by_name) { + auto res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(3u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "changed"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"new"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"changed"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + full, + {"deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, empty) { + auto res = rclcpp::ParameterEventsFilter( + empty, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(0u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, singular) { + auto res = rclcpp::ParameterEventsFilter( + np, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + cp, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + dp, + {"new", "changed", "deleted"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, multiple) { + auto res = rclcpp::ParameterEventsFilter( + multiple, + {"new", "new2"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + multiple, + {"new2"}, + {nt, ct, dt}); + EXPECT_EQ(1u, res.get_events().size()); + res = rclcpp::ParameterEventsFilter( + multiple, + {"new", "new2"}, + {ct, dt}); + EXPECT_EQ(0u, res.get_events().size()); +} + +TEST_F(TestParameterEventFilter, validate_data) { + auto res = rclcpp::ParameterEventsFilter( + multiple, + {"new", "new2"}, + {nt, ct, dt}); + EXPECT_EQ(2u, res.get_events().size()); + EXPECT_EQ(nt, res.get_events()[0].first); + EXPECT_EQ(nt, res.get_events()[1].first); + EXPECT_EQ(1, res.get_events()[0].second->value.integer_value); + EXPECT_EQ(2, res.get_events()[1].second->value.integer_value); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_parameter_map.cpp b/rclcpp-eloquent/rclcpp/test/test_parameter_map.cpp new file mode 100644 index 0000000..3f54e4c --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_parameter_map.cpp @@ -0,0 +1,355 @@ +// Copyright 2018 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 +#include +#include + +#include +#include +#include + +#include "rclcpp/parameter_map.hpp" + +rcl_params_t * +make_params(std::vector node_names) +{ + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_params_t * c_params = rcl_yaml_node_struct_init(alloc); + c_params->num_nodes = node_names.size(); + c_params->allocator = alloc; + if (c_params->num_nodes) { + // Copy node names + for (size_t n = 0; n < node_names.size(); ++n) { + c_params->node_names[n] = static_cast(alloc.allocate( + sizeof(char) * (node_names[n].size() + 1), alloc.state)); + std::snprintf(c_params->node_names[n], node_names[n].size() + 1, "%s", node_names[n].c_str()); + } + // zero init node params + for (size_t n = 0; n < node_names.size(); ++n) { + c_params->params[n].parameter_names = NULL; + c_params->params[n].parameter_values = NULL; + c_params->params[n].num_params = 0; + } + } + return c_params; +} + +void +make_node_params(rcl_params_t * c_params, size_t node_idx, std::vector param_names) +{ + rcl_allocator_t alloc = c_params->allocator; + ASSERT_LT(node_idx, c_params->num_nodes); + ASSERT_GT(param_names.size(), 0u); + + rcl_node_params_s * c_node_params = &(c_params->params[node_idx]); + c_node_params->num_params = param_names.size(); + + // Copy parameter names + c_node_params->parameter_names = static_cast( + alloc.allocate(sizeof(char *) * param_names.size(), alloc.state)); + for (size_t p = 0; p < param_names.size(); ++p) { + const std::string & param_name = param_names[p]; + c_node_params->parameter_names[p] = static_cast(alloc.allocate( + sizeof(char) * (param_name.size() + 1), alloc.state)); + std::snprintf( + c_node_params->parameter_names[p], param_name.size() + 1, "%s", param_name.c_str()); + } + // zero init parameter value + c_node_params->parameter_values = static_cast(alloc.allocate( + sizeof(rcl_variant_t) * param_names.size(), alloc.state)); + for (size_t p = 0; p < param_names.size(); ++p) { + c_node_params->parameter_values[p].bool_value = NULL; + c_node_params->parameter_values[p].integer_value = NULL; + c_node_params->parameter_values[p].double_value = NULL; + c_node_params->parameter_values[p].string_value = NULL; + c_node_params->parameter_values[p].byte_array_value = NULL; + c_node_params->parameter_values[p].bool_array_value = NULL; + c_node_params->parameter_values[p].integer_array_value = NULL; + c_node_params->parameter_values[p].double_array_value = NULL; + c_node_params->parameter_values[p].string_array_value = NULL; + } +} + +TEST(Test_parameter_map_from, null_c_parameter) +{ + EXPECT_THROW(rclcpp::parameter_map_from(NULL), rclcpp::exceptions::InvalidParametersException); +} + +TEST(Test_parameter_map_from, null_node_names) +{ + rcl_params_t * c_params = make_params({}); + c_params->num_nodes = 1; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->num_nodes = 0; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_params) +{ + rcl_params_t * c_params = make_params({"foo"}); + std::snprintf(c_params->node_names[0], 3 + 1, "foo"); + auto allocated_params = c_params->params; + c_params->params = NULL; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->params = allocated_params; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_name_in_node_names) +{ + rcl_params_t * c_params = make_params({"foo"}); + auto allocated_name = c_params->node_names[0]; + c_params->node_names[0] = NULL; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->node_names[0] = allocated_name; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"bar"}); + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParameterValueException); + + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_param_name) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"bar"}); + auto allocated_name = c_params->params[0].parameter_names[0]; + c_params->params[0].parameter_names[0] = NULL; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->params[0].parameter_names[0] = allocated_name; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, bool_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"true_bool", "false_bool"}); + bool true_bool = true; + bool false_bool = false; + c_params->params[0].parameter_values[0].bool_value = &true_bool; + c_params->params[0].parameter_values[1].bool_value = &false_bool; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("true_bool", params.at(0).get_name().c_str()); + EXPECT_TRUE(params.at(0).get_value()); + EXPECT_STREQ("false_bool", params.at(1).get_name().c_str()); + EXPECT_FALSE(params.at(1).get_value()); + + c_params->params[0].parameter_values[0].bool_value = NULL; + c_params->params[0].parameter_values[1].bool_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, integer_param_value) +{ + rcl_params_t * c_params = make_params({"bar"}); + make_node_params(c_params, 0, {"positive.int", "negative.int"}); + int64_t positive_int = 42; + int64_t negative_int = -12345; + c_params->params[0].parameter_values[0].integer_value = &positive_int; + c_params->params[0].parameter_values[1].integer_value = &negative_int; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/bar"); + EXPECT_STREQ("positive.int", params.at(0).get_name().c_str()); + EXPECT_EQ(42, params.at(0).get_value()); + EXPECT_STREQ("negative.int", params.at(1).get_name().c_str()); + EXPECT_EQ(-12345, params.at(1).get_value()); + + c_params->params[0].parameter_values[0].integer_value = NULL; + c_params->params[0].parameter_values[1].integer_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, double_param_value) +{ + rcl_params_t * c_params = make_params({"foo/bar"}); + make_node_params(c_params, 0, {"positive.double", "negative.double"}); + double positive_double = 3.14; + double negative_double = -2.718; + c_params->params[0].parameter_values[0].double_value = &positive_double; + c_params->params[0].parameter_values[1].double_value = &negative_double; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo/bar"); + EXPECT_STREQ("positive.double", params.at(0).get_name().c_str()); + EXPECT_DOUBLE_EQ(3.14, params.at(0).get_value()); + EXPECT_STREQ("negative.double", params.at(1).get_name().c_str()); + EXPECT_DOUBLE_EQ(-2.718, params.at(1).get_value()); + + c_params->params[0].parameter_values[0].double_value = NULL; + c_params->params[0].parameter_values[1].double_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, string_param_value) +{ + rcl_params_t * c_params = make_params({"/foo/bar"}); + make_node_params(c_params, 0, {"string_param"}); + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[0].parameter_values[0].string_value = c_hello_world; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo/bar"); + EXPECT_STREQ("string_param", params.at(0).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value().c_str()); + + c_params->params[0].parameter_values[0].string_value = NULL; + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} + +#define MAKE_ARRAY_VALUE(VAR, TYPE, V1, V2) \ + do { \ + VAR.values = new TYPE[2]; \ + VAR.size = 2; \ + VAR.values[0] = V1; \ + VAR.values[1] = V2; \ + } while (false) + +#define FREE_ARRAY_VALUE(VAR) \ + do { \ + delete[] VAR.values; \ + } while (false) + +TEST(Test_parameter_map_from, byte_array_param_value) +{ + rcl_params_t * c_params = make_params({"/foobar"}); + make_node_params(c_params, 0, {"byte_array_param"}); + rcl_byte_array_t c_byte_array; + MAKE_ARRAY_VALUE(c_byte_array, uint8_t, 0xf0, 0xaa); + c_params->params[0].parameter_values[0].byte_array_value = &c_byte_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foobar"); + EXPECT_STREQ("byte_array_param", params.at(0).get_name().c_str()); + std::vector byte_array = params.at(0).get_value>(); + ASSERT_EQ(2u, byte_array.size()); + EXPECT_EQ(0xf0, byte_array.at(0)); + EXPECT_EQ(0xaa, byte_array.at(1)); + + c_params->params[0].parameter_values[0].byte_array_value = NULL; + FREE_ARRAY_VALUE(c_byte_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, bool_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo/bar/baz"}); + make_node_params(c_params, 0, {"bool_array_param"}); + rcl_bool_array_t c_bool_array; + MAKE_ARRAY_VALUE(c_bool_array, bool, true, false); + c_params->params[0].parameter_values[0].bool_array_value = &c_bool_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo/bar/baz"); + EXPECT_STREQ("bool_array_param", params.at(0).get_name().c_str()); + std::vector bool_array = params.at(0).get_value>(); + ASSERT_EQ(2u, bool_array.size()); + EXPECT_TRUE(bool_array.at(0)); + EXPECT_FALSE(bool_array.at(1)); + + c_params->params[0].parameter_values[0].bool_array_value = NULL; + FREE_ARRAY_VALUE(c_bool_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, integer_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"integer_array_param"}); + rcl_int64_array_t c_integer_array; + MAKE_ARRAY_VALUE(c_integer_array, int64_t, 42, -12345); + c_params->params[0].parameter_values[0].integer_array_value = &c_integer_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("integer_array_param", params.at(0).get_name().c_str()); + std::vector integer_array = params.at(0).get_value>(); + ASSERT_EQ(2u, integer_array.size()); + EXPECT_EQ(42, integer_array.at(0)); + EXPECT_EQ(-12345, integer_array.at(1)); + + c_params->params[0].parameter_values[0].integer_array_value = NULL; + FREE_ARRAY_VALUE(c_integer_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, double_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"double_array_param"}); + rcl_double_array_t c_double_array; + MAKE_ARRAY_VALUE(c_double_array, double, 3.14, -2.718); + c_params->params[0].parameter_values[0].double_array_value = &c_double_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("double_array_param", params.at(0).get_name().c_str()); + std::vector double_array = params.at(0).get_value>(); + ASSERT_EQ(2u, double_array.size()); + EXPECT_DOUBLE_EQ(3.14, double_array.at(0)); + EXPECT_DOUBLE_EQ(-2.718, double_array.at(1)); + + c_params->params[0].parameter_values[0].double_array_value = NULL; + FREE_ARRAY_VALUE(c_double_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, string_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"string_array_param"}); + rcutils_string_array_t c_string_array = rcutils_get_zero_initialized_string_array(); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_string_array_init(&c_string_array, 2, &(c_params->allocator))); + c_string_array.data[0] = rcutils_strdup("Hello", c_params->allocator); + c_string_array.data[1] = rcutils_strdup("World", c_params->allocator); + c_params->params[0].parameter_values[0].string_array_value = &c_string_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("string_array_param", params.at(0).get_name().c_str()); + std::vector string_array = params.at(0).get_value>(); + ASSERT_EQ(2u, string_array.size()); + EXPECT_STREQ("Hello", string_array.at(0).c_str()); + EXPECT_STREQ("World", string_array.at(1).c_str()); + + EXPECT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&c_string_array)); + c_params->params[0].parameter_values[0].string_array_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_publisher.cpp b/rclcpp-eloquent/rclcpp/test/test_publisher.cpp new file mode 100644 index 0000000..6719197 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_publisher.cpp @@ -0,0 +1,205 @@ +// Copyright 2017 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +class TestPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::is_initialized()) { + rclcpp::init(0, nullptr); + } + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("my_node", "/ns", node_options); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +struct TestParameters +{ + TestParameters(rclcpp::QoS qos, const std::string & description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestPublisherInvalidIntraprocessQos + : public TestPublisher, + public ::testing::WithParamInterface +{}; + +class TestPublisherSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing publisher construction and destruction. + */ +TEST_F(TestPublisher, construction_and_destruction) { + initialize(); + using rcl_interfaces::msg::IntraProcessMessage; + { + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + + { + ASSERT_THROW( + { + auto publisher = node->create_publisher("invalid_topic?", 42); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +/* + Testing publisher creation signatures. + */ +TEST_F(TestPublisher, various_creation_signatures) { + initialize(); + using rcl_interfaces::msg::IntraProcessMessage; + { + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + { + auto publisher = node->create_publisher("topic", rclcpp::QoS(42)); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", rclcpp::QoS(rclcpp::KeepLast(42))); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", rclcpp::QoS(rclcpp::KeepAll())); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + { + auto publisher = + rclcpp::create_publisher(node, "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } +} + +/* + Testing publisher with intraprocess enabled and invalid QoS + */ +TEST_P(TestPublisherInvalidIntraprocessQos, test_publisher_throws) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::QoS qos = GetParam().qos; + using rcl_interfaces::msg::IntraProcessMessage; + { + ASSERT_THROW( + {auto publisher = node->create_publisher("topic", qos);}, + std::invalid_argument); + } +} + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(2); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), + "transient_local_qos")); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + + return parameters; +} + +INSTANTIATE_TEST_CASE_P( + TestPublisherThrows, TestPublisherInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); + +/* + Testing publisher construction and destruction for subnodes. + */ +TEST_F(TestPublisherSub, construction_and_destruction) { + using rcl_interfaces::msg::IntraProcessMessage; + { + auto publisher = subnode->create_publisher("topic", 42); + + EXPECT_STREQ(publisher->get_topic_name(), "/ns/sub_ns/topic"); + } + + { + auto publisher = subnode->create_publisher("/topic", 42); + + EXPECT_STREQ(publisher->get_topic_name(), "/topic"); + } + + { + ASSERT_THROW( + { + auto publisher = subnode->create_publisher("invalid_topic?", 42); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} diff --git a/rclcpp-eloquent/rclcpp/test/test_publisher_subscription_count_api.cpp b/rclcpp-eloquent/rclcpp/test/test_publisher_subscription_count_api.cpp new file mode 100644 index 0000000..d92f8f3 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_publisher_subscription_count_api.cpp @@ -0,0 +1,178 @@ +// Copyright 2019 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +using rcl_interfaces::msg::IntraProcessMessage; + +/** + * Parameterized test. + * The first param are the NodeOptions used to create the nodes. + * The second param are the expect intraprocess count results. + */ +struct TestParameters +{ + rclcpp::NodeOptions node_options[2]; + uint64_t intraprocess_count_results[2]; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestPublisherSubscriptionCount : public ::testing::TestWithParam +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + +protected: + void SetUp() {} + + void TearDown() {} + + static std::chrono::milliseconds offset; +}; + +std::chrono::milliseconds TestPublisherSubscriptionCount::offset = std::chrono::milliseconds(2000); + +void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg) +{ + (void)msg; +} + +TEST_P(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) +{ + TestParameters parameters = GetParam(); + rclcpp::Node::SharedPtr node = std::make_shared( + "my_node", + "/ns", + parameters.node_options[0]); + auto publisher = node->create_publisher("/topic", 10); + + EXPECT_EQ(publisher->get_subscription_count(), 0u); + EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u); + { + auto sub = node->create_subscription("/topic", 10, &OnMessage); + rclcpp::sleep_for(offset); + EXPECT_EQ(publisher->get_subscription_count(), 1u); + EXPECT_EQ( + publisher->get_intra_process_subscription_count(), + parameters.intraprocess_count_results[0]); + { + rclcpp::Node::SharedPtr another_node = std::make_shared( + "another_node", + "/ns", + parameters.node_options[1]); + auto another_sub = + another_node->create_subscription("/topic", 10, &OnMessage); + + rclcpp::sleep_for(offset); + EXPECT_EQ(publisher->get_subscription_count(), 2u); + EXPECT_EQ( + publisher->get_intra_process_subscription_count(), + parameters.intraprocess_count_results[1]); + } + rclcpp::sleep_for(offset); + EXPECT_EQ(publisher->get_subscription_count(), 1u); + EXPECT_EQ( + publisher->get_intra_process_subscription_count(), + parameters.intraprocess_count_results[0]); + } + /** + * Counts should be zero here, as all are subscriptions are out of scope. + * Subscriptions count checking is always preceeded with an sleep, as random failures had been + * detected without it. */ + rclcpp::sleep_for(offset); + EXPECT_EQ(publisher->get_subscription_count(), 0u); + EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u); +} + +auto get_new_context() +{ + auto context = rclcpp::Context::make_shared(); + context->init(0, nullptr); + return context; +} + +TestParameters parameters[] = { + /* + Testing publisher subscription count api and internal process subscription count. + Two subscriptions in the same topic, both using intraprocess comm. + */ + { + { + rclcpp::NodeOptions().use_intra_process_comms(true), + rclcpp::NodeOptions().use_intra_process_comms(true) + }, + {1u, 2u}, + "two_subscriptions_intraprocess_comm" + }, + /* + Testing publisher subscription count api and internal process subscription count. + Two subscriptions, one using intra-process comm and the other not using it. + */ + { + { + rclcpp::NodeOptions().use_intra_process_comms(true), + rclcpp::NodeOptions().use_intra_process_comms(false) + }, + {1u, 1u}, + "two_subscriptions_one_intraprocess_one_not" + }, + /* + Testing publisher subscription count api and internal process subscription count. + Two contexts, both using intra-process. + */ + { + { + rclcpp::NodeOptions().use_intra_process_comms(true), + rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(true) + }, + {1u, 1u}, + "two_subscriptions_in_two_contexts_with_intraprocess_comm" + }, + /* + Testing publisher subscription count api and internal process subscription count. + Two contexts, both of them not using intra-process comm. + */ + { + { + rclcpp::NodeOptions().use_intra_process_comms(false), + rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(false) + }, + {0u, 0u}, + "two_subscriptions_in_two_contexts_without_intraprocess_comm" + } +}; + +INSTANTIATE_TEST_CASE_P( + TestWithDifferentNodeOptions, TestPublisherSubscriptionCount, + ::testing::ValuesIn(parameters), + ::testing::PrintToStringParamName()); diff --git a/rclcpp-eloquent/rclcpp/test/test_rate.cpp b/rclcpp-eloquent/rclcpp/test/test_rate.cpp new file mode 100644 index 0000000..a4c1c43 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_rate.cpp @@ -0,0 +1,100 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/rate.hpp" + +/* + Basic tests for the Rate and WallRate classes. + */ +TEST(TestRate, rate_basics) { + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + auto start = std::chrono::system_clock::now(); + rclcpp::Rate r(period); + ASSERT_FALSE(r.is_steady()); + ASSERT_TRUE(r.sleep()); + auto one = std::chrono::system_clock::now(); + auto delta = one - start; + ASSERT_TRUE(period < delta); + ASSERT_TRUE(period * overrun_ratio > delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(r.sleep()); + auto two = std::chrono::system_clock::now(); + delta = two - start; + ASSERT_TRUE(2 * period < delta); + ASSERT_TRUE(2 * period * overrun_ratio > delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::system_clock::now(); + r.reset(); + ASSERT_TRUE(r.sleep()); + auto three = std::chrono::system_clock::now(); + delta = three - two_offset; + ASSERT_TRUE(period < delta); + ASSERT_TRUE(period * overrun_ratio > delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::system_clock::now(); + ASSERT_FALSE(r.sleep()); + auto five = std::chrono::system_clock::now(); + delta = five - four; + ASSERT_TRUE(epsilon > delta); +} + +TEST(TestRate, wall_rate_basics) { + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + auto start = std::chrono::steady_clock::now(); + rclcpp::WallRate r(period); + ASSERT_TRUE(r.is_steady()); + ASSERT_TRUE(r.sleep()); + auto one = std::chrono::steady_clock::now(); + auto delta = one - start; + ASSERT_TRUE(period < delta); + ASSERT_TRUE(period * overrun_ratio > delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(r.sleep()); + auto two = std::chrono::steady_clock::now(); + delta = two - start; + ASSERT_TRUE(2 * period < delta + epsilon); + ASSERT_TRUE(2 * period * overrun_ratio > delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::steady_clock::now(); + r.reset(); + ASSERT_TRUE(r.sleep()); + auto three = std::chrono::steady_clock::now(); + delta = three - two_offset; + ASSERT_TRUE(period < delta); + ASSERT_TRUE(period * overrun_ratio > delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::steady_clock::now(); + ASSERT_FALSE(r.sleep()); + auto five = std::chrono::steady_clock::now(); + delta = five - four; + ASSERT_TRUE(epsilon > delta); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_ring_buffer_implementation.cpp b/rclcpp-eloquent/rclcpp/test/test_ring_buffer_implementation.cpp new file mode 100644 index 0000000..07dfd8d --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_ring_buffer_implementation.cpp @@ -0,0 +1,81 @@ +// Copyright 2019 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 + +#include "gtest/gtest.h" + +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" + +/* + Construtctor + */ +TEST(TestRingBufferImplementation, constructor) { + // Cannot create a buffer of size zero. + EXPECT_THROW( + rclcpp::experimental::buffers::RingBufferImplementation rb(0), + std::invalid_argument); + + rclcpp::experimental::buffers::RingBufferImplementation rb(1); + + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} + +/* + Basic usage + - insert data and check that it has data + - extract data + - overwrite old data writing over the buffer capacity + */ +TEST(TestRingBufferImplementation, basic_usage) { + rclcpp::experimental::buffers::RingBufferImplementation rb(2); + + rb.enqueue('a'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + char v = rb.dequeue(); + + EXPECT_EQ('a', v); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + rb.enqueue('b'); + rb.enqueue('c'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + rb.enqueue('d'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + v = rb.dequeue(); + + EXPECT_EQ('c', v); + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + v = rb.dequeue(); + + EXPECT_EQ('d', v); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_serialized_message_allocator.cpp b/rclcpp-eloquent/rclcpp/test/test_serialized_message_allocator.cpp new file mode 100644 index 0000000..bc17772 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_serialized_message_allocator.cpp @@ -0,0 +1,68 @@ +// Copyright 2018 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 +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rcl/types.h" + +#include "test_msgs/msg/empty.hpp" + +TEST(TestSerializedMessageAllocator, default_allocator) { + using DummyMessageT = float; + auto mem_strategy = + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + + auto msg0 = mem_strategy->borrow_serialized_message(); + ASSERT_EQ(msg0->buffer_capacity, 0u); + mem_strategy->return_serialized_message(msg0); + + auto msg100 = mem_strategy->borrow_serialized_message(100); + ASSERT_EQ(msg100->buffer_capacity, 100u); + mem_strategy->return_serialized_message(msg100); + + auto msg200 = mem_strategy->borrow_serialized_message(); + auto ret = rmw_serialized_message_resize(msg200.get(), 200); + ASSERT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(0u, msg200->buffer_length); + EXPECT_EQ(200u, msg200->buffer_capacity); + mem_strategy->return_serialized_message(msg200); + + auto msg1000 = mem_strategy->borrow_serialized_message(1000); + ASSERT_EQ(msg1000->buffer_capacity, 1000u); + ret = rmw_serialized_message_resize(msg1000.get(), 2000); + ASSERT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(2000u, msg1000->buffer_capacity); + mem_strategy->return_serialized_message(msg1000); +} + +TEST(TestSerializedMessageAllocator, borrow_from_subscription) { + rclcpp::init(0, NULL); + + auto node = std::make_shared("test_serialized_message_allocator_node"); + std::shared_ptr sub = + node->create_subscription( + "~/dummy_topic", 10, + [](std::shared_ptr test_msg) {(void) test_msg;}); + + auto msg0 = sub->create_serialized_message(); + EXPECT_EQ(0u, msg0->buffer_capacity); + sub->return_serialized_message(msg0); + + rclcpp::shutdown(); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_service.cpp b/rclcpp-eloquent/rclcpp/test/test_service.cpp new file mode 100644 index 0000000..9ab90e1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_service.cpp @@ -0,0 +1,107 @@ +// Copyright 2017 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 +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +class TestService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestServiceSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestService, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + auto callback = + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { + }; + { + auto service = node->create_service("service", callback); + } + + { + ASSERT_THROW( + { + auto service = node->create_service("invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +/* + Testing service construction and destruction for subnodes. + */ +TEST_F(TestServiceSub, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + auto callback = + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { + }; + { + auto service = subnode->create_service("service", callback); + EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service"); + } + + { + ASSERT_THROW( + { + auto service = node->create_service("invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp-eloquent/rclcpp/test/test_subscription.cpp b/rclcpp-eloquent/rclcpp/test/test_subscription.cpp new file mode 100644 index 0000000..5a4d334 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_subscription.cpp @@ -0,0 +1,297 @@ +// Copyright 2017 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +class TestSubscription : public ::testing::Test +{ +public: + void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg) + { + (void)msg; + } + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("test_subscription", "/ns", node_options); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +struct TestParameters +{ + TestParameters(rclcpp::QoS qos, std::string description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestSubscriptionInvalidIntraprocessQos + : public TestSubscription, + public ::testing::WithParamInterface +{}; + +class TestSubscriptionSub : public ::testing::Test +{ +public: + void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg) + { + (void)msg; + } + +protected: + static void SetUpTestCase() + { + } + + void SetUp() + { + node = std::make_shared("test_subscription", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +class SubscriptionClassNodeInheritance : public rclcpp::Node +{ +public: + SubscriptionClassNodeInheritance() + : Node("subscription_class_node_inheritance") + { + } + + void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg) + { + (void)msg; + } + + void CreateSubscription() + { + auto callback = std::bind( + &SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1); + using rcl_interfaces::msg::IntraProcessMessage; + auto sub = this->create_subscription("topic", 10, callback); + } +}; + +class SubscriptionClass +{ +public: + void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg) + { + (void)msg; + } + + void CreateSubscription() + { + auto node = std::make_shared("test_subscription_member_callback", "/ns"); + auto callback = std::bind(&SubscriptionClass::OnMessage, this, std::placeholders::_1); + using rcl_interfaces::msg::IntraProcessMessage; + auto sub = node->create_subscription("topic", 10, callback); + } +}; + +/* + Testing subscription construction and destruction. + */ +TEST_F(TestSubscription, construction_and_destruction) { + initialize(); + using rcl_interfaces::msg::IntraProcessMessage; + auto callback = [](const IntraProcessMessage::SharedPtr msg) { + (void)msg; + }; + { + auto sub = node->create_subscription("topic", 10, callback); + } + + { + ASSERT_THROW( + { + auto sub = node->create_subscription("invalid_topic?", 10, callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +/* + Testing subscription construction and destruction for subnodes. + */ +TEST_F(TestSubscriptionSub, construction_and_destruction) { + using rcl_interfaces::msg::IntraProcessMessage; + auto callback = [](const IntraProcessMessage::SharedPtr msg) { + (void)msg; + }; + { + auto sub = subnode->create_subscription("topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); + } + + { + auto sub = subnode->create_subscription("/topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/topic"); + } + + { + auto sub = subnode->create_subscription("~/topic", 1, callback); + std::string expected_topic_name = + std::string(node->get_namespace()) + "/" + node->get_name() + "/topic"; + EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); + } + + { + ASSERT_THROW( + { + auto sub = node->create_subscription("invalid_topic?", 1, callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +/* + Testing subscription creation signatures. + */ +TEST_F(TestSubscription, various_creation_signatures) { + initialize(); + using rcl_interfaces::msg::IntraProcessMessage; + auto cb = [](rcl_interfaces::msg::IntraProcessMessage::SharedPtr) {}; + { + auto sub = node->create_subscription("topic", 1, cb); + (void)sub; + } + { + auto sub = node->create_subscription("topic", rclcpp::QoS(1), cb); + (void)sub; + } + { + auto sub = + node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); + (void)sub; + } + { + auto sub = + node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); + (void)sub; + } + { + auto sub = node->create_subscription( + "topic", 42, cb, rclcpp::SubscriptionOptions()); + (void)sub; + } + { + auto sub = rclcpp::create_subscription( + node, "topic", 42, cb, rclcpp::SubscriptionOptions()); + (void)sub; + } +} + +/* + Testing subscriptions using std::bind. + */ +TEST_F(TestSubscription, callback_bind) { + initialize(); + using rcl_interfaces::msg::IntraProcessMessage; + { + // Member callback for plain class + SubscriptionClass subscriptionObject; + subscriptionObject.CreateSubscription(); + } + { + // Member callback for class inheriting from rclcpp::Node + SubscriptionClassNodeInheritance subscriptionObject; + subscriptionObject.CreateSubscription(); + } + { + // Member callback for class inheriting from testing::Test + // Regression test for https://github.com/ros2/rclcpp/issues/479 where the TEST_F GTest macro + // was interfering with rclcpp's `function_traits`. + auto callback = std::bind(&TestSubscription::OnMessage, this, std::placeholders::_1); + auto sub = node->create_subscription("topic", 1, callback); + } +} + +/* + Testing subscription with intraprocess enabled and invalid QoS + */ +TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::QoS qos = GetParam().qos; + using rcl_interfaces::msg::IntraProcessMessage; + { + auto callback = std::bind( + &TestSubscriptionInvalidIntraprocessQos::OnMessage, + this, + std::placeholders::_1); + + ASSERT_THROW( + {auto subscription = node->create_subscription( + "topic", + qos, + callback);}, + std::invalid_argument); + } +} + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(3); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), + "transient_local_qos")); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + + return parameters; +} + +INSTANTIATE_TEST_CASE_P( + TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); diff --git a/rclcpp-eloquent/rclcpp/test/test_subscription_publisher_count_api.cpp b/rclcpp-eloquent/rclcpp/test/test_subscription_publisher_count_api.cpp new file mode 100644 index 0000000..cdf1bf9 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_subscription_publisher_count_api.cpp @@ -0,0 +1,123 @@ +// Copyright 2019 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +using rcl_interfaces::msg::IntraProcessMessage; + +struct TestParameters +{ + rclcpp::NodeOptions node_options; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestSubscriptionPublisherCount : public ::testing::TestWithParam +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + +protected: + void SetUp() {} + void TearDown() {} + static std::chrono::milliseconds offset; +}; + +std::chrono::milliseconds TestSubscriptionPublisherCount::offset = std::chrono::milliseconds(2000); + +void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg) +{ + (void)msg; +} + +TEST_P(TestSubscriptionPublisherCount, increasing_and_decreasing_counts) +{ + rclcpp::NodeOptions node_options = GetParam().node_options; + rclcpp::Node::SharedPtr node = std::make_shared( + "my_node", + "/ns", + node_options); + auto subscription = node->create_subscription("/topic", 10, &OnMessage); + + EXPECT_EQ(subscription->get_publisher_count(), 0u); + { + auto pub = node->create_publisher("/topic", 10); + rclcpp::sleep_for(offset); + EXPECT_EQ(subscription->get_publisher_count(), 1u); + { + rclcpp::Node::SharedPtr another_node = std::make_shared( + "another_node", + "/ns", + node_options); + auto another_pub = + another_node->create_publisher("/topic", 10); + + rclcpp::sleep_for(offset); + EXPECT_EQ(subscription->get_publisher_count(), 2u); + } + rclcpp::sleep_for(offset); + EXPECT_EQ(subscription->get_publisher_count(), 1u); + } + rclcpp::sleep_for(offset); + EXPECT_EQ(subscription->get_publisher_count(), 0u); +} + +auto get_new_context() +{ + auto context = rclcpp::Context::make_shared(); + context->init(0, nullptr); + return context; +} + +TestParameters parameters[] = { + /* + Testing subscription publisher count api. + One context. + */ + { + rclcpp::NodeOptions(), + "one_context_test" + }, + /* + Testing subscription publisher count api. + Two contexts. + */ + { + rclcpp::NodeOptions().context(get_new_context()), + "two_contexts_test" + } +}; + +INSTANTIATE_TEST_CASE_P( + TestWithDifferentNodeOptions, + TestSubscriptionPublisherCount, + testing::ValuesIn(parameters), + testing::PrintToStringParamName()); diff --git a/rclcpp-eloquent/rclcpp/test/test_subscription_traits.cpp b/rclcpp-eloquent/rclcpp/test/test_subscription_traits.cpp new file mode 100644 index 0000000..3112caf --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_subscription_traits.cpp @@ -0,0 +1,178 @@ +// Copyright 2017 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 +#include +#include + +#include "rcl/types.h" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/subscription_traits.hpp" + +#include "test_msgs/msg/empty.hpp" + +void serialized_callback_copy(rcl_serialized_message_t unused) +{ + (void) unused; +} + +void serialized_callback_shared_ptr(std::shared_ptr unused) +{ + (void) unused; +} + +void not_serialized_callback(char * unused) +{ + (void) unused; +} + +void not_serialized_shared_ptr_callback(std::shared_ptr unused) +{ + (void) unused; +} + +void not_serialized_unique_ptr_callback( + test_msgs::msg::Empty::UniquePtrWithDeleter, + test_msgs::msg::Empty>> unused) +{ + (void) unused; +} + +TEST(TestSubscriptionTraits, is_serialized_callback) { + // Test regular functions + auto cb1 = &serialized_callback_copy; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == true, + "rcl_serialized_message_t in a first argument callback makes it a serialized callback"); + + auto cb2 = &serialized_callback_shared_ptr; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == true, + "std::shared_ptr in a callback makes it a serialized callback"); + + auto cb3 = ¬_serialized_callback; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a char * is not a serialized callback"); + + auto cb4 = ¬_serialized_shared_ptr_callback; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::shared_tr is not a serialized callback"); + + auto cb5 = [](rcl_serialized_message_t unused) -> void + { + (void) unused; + }; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == true, + "rcl_serialized_message_t in a first argument callback makes it a serialized callback"); + + using MessageT = test_msgs::msg::Empty; + using MessageTAllocator = std::allocator; + using MessageTDeallocator = rclcpp::allocator::Deleter; + auto cb6 = [](MessageT::UniquePtrWithDeleter unique_msg_ptr) -> void + { + (void) unique_msg_ptr; + }; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::unique_ptr of test_msgs::msg::Empty is not a serialized callback"); + + auto cb7 = ¬_serialized_unique_ptr_callback; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback"); +} + +TEST(TestSubscriptionTraits, callback_messages) { + static_assert( + std::is_same< + std::shared_ptr, + rclcpp::function_traits::function_traits< + decltype(not_serialized_shared_ptr_callback) + >::template argument_type<0> + >::value, "wrong!"); + + static_assert( + std::is_same< + char, + rclcpp::subscription_traits::extract_message_type< + rclcpp::function_traits::function_traits< + decltype(not_serialized_shared_ptr_callback) + >::template argument_type<0> + >::type + >::value, "wrong!"); + + auto cb1 = &serialized_callback_copy; + static_assert( + std::is_same< + rcl_serialized_message_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "serialized callback message type is rcl_serialized_message_t"); + + auto cb2 = &serialized_callback_shared_ptr; + static_assert( + std::is_same< + rcl_serialized_message_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "serialized callback message type is rcl_serialized_message_t"); + + auto cb3 = ¬_serialized_callback; + static_assert( + std::is_same< + char *, + rclcpp::subscription_traits::has_message_type::type>::value, + "not serialized callback message type is char"); + + auto cb4 = ¬_serialized_shared_ptr_callback; + static_assert( + std::is_same< + char, + rclcpp::subscription_traits::has_message_type::type>::value, + "not serialized shared_ptr callback message type is std::shared_ptr"); + + auto cb5 = [](rcl_serialized_message_t unused) -> void + { + (void) unused; + }; + static_assert( + std::is_same< + rcl_serialized_message_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "serialized callback message type is rcl_serialized_message_t"); + + using MessageT = test_msgs::msg::Empty; + using MessageTAllocator = std::allocator; + using MessageTDeallocator = rclcpp::allocator::Deleter; + auto cb6 = [](std::unique_ptr unique_msg_ptr) -> void + { + (void) unique_msg_ptr; + }; + static_assert( + std::is_same< + test_msgs::msg::Empty, + rclcpp::subscription_traits::has_message_type::type>::value, + "passing a std::unique_ptr of test_msgs::msg::Empty has message type Empty"); + + auto cb7 = ¬_serialized_unique_ptr_callback; + static_assert( + std::is_same< + test_msgs::msg::Empty, + rclcpp::subscription_traits::has_message_type::type>::value, + "passing a fancy std::unique_ptr of test_msgs::msg::Empty has message type Empty"); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_time.cpp b/rclcpp-eloquent/rclcpp/test/test_time.cpp new file mode 100644 index 0000000..22c9772 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_time.cpp @@ -0,0 +1,254 @@ +// Copyright 2017 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 +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" + +namespace +{ + +bool logical_eq(const bool a, const bool b) +{ + return (a && b) || ((!a) && !(b)); +} + +} // namespace + + +class TestTime : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST(TestTime, clock_type_access) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type()); + + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + EXPECT_EQ(RCL_SYSTEM_TIME, system_clock.get_clock_type()); + + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type()); +} + +TEST(TestTime, time_sources) { + using builtin_interfaces::msg::Time; + rclcpp::Clock ros_clock(RCL_ROS_TIME); + Time ros_now = ros_clock.now(); + EXPECT_NE(0, ros_now.sec); + EXPECT_NE(0u, ros_now.nanosec); + + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + Time system_now = system_clock.now(); + EXPECT_NE(0, system_now.sec); + EXPECT_NE(0u, system_now.nanosec); + + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + Time steady_now = steady_clock.now(); + EXPECT_NE(0, steady_now.sec); + EXPECT_NE(0u, steady_now.nanosec); +} + +TEST(TestTime, conversions) { + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + + rclcpp::Time now = system_clock.now(); + builtin_interfaces::msg::Time now_msg = now; + + rclcpp::Time now_again = now_msg; + EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds()); + + builtin_interfaces::msg::Time msg; + msg.sec = 12345; + msg.nanosec = 67890; + + rclcpp::Time time = msg; + EXPECT_EQ( + RCL_S_TO_NS(static_cast(msg.sec)) + static_cast(msg.nanosec), + time.nanoseconds()); + EXPECT_EQ(static_cast(msg.sec), RCL_NS_TO_S(time.nanoseconds())); + + builtin_interfaces::msg::Time negative_time_msg; + negative_time_msg.sec = -1; + negative_time_msg.nanosec = 1; + + EXPECT_ANY_THROW( + { + rclcpp::Time negative_time = negative_time_msg; + }); + + EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); + + EXPECT_ANY_THROW( + { + rclcpp::Time assignment(1, 2); + assignment = negative_time_msg; + }); +} + +TEST(TestTime, operators) { + rclcpp::Time old(1, 0); + rclcpp::Time young(2, 0); + + EXPECT_TRUE(old < young); + EXPECT_TRUE(young > old); + EXPECT_TRUE(old <= young); + EXPECT_TRUE(young >= old); + EXPECT_FALSE(young == old); + EXPECT_TRUE(young != old); + + rclcpp::Duration sub = young - old; + EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds())); + EXPECT_EQ(sub, young - old); + + rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME); + rclcpp::Time steady_time(0, 0, RCL_STEADY_TIME); + + EXPECT_ANY_THROW((void)(system_time == steady_time)); + EXPECT_ANY_THROW((void)(system_time != steady_time)); + EXPECT_ANY_THROW((void)(system_time <= steady_time)); + EXPECT_ANY_THROW((void)(system_time >= steady_time)); + EXPECT_ANY_THROW((void)(system_time < steady_time)); + EXPECT_ANY_THROW((void)(system_time > steady_time)); + EXPECT_ANY_THROW((void)(system_time - steady_time)); + + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + + rclcpp::Time now = system_clock.now(); + rclcpp::Time later = steady_clock.now(); + + EXPECT_ANY_THROW((void)(now == later)); + EXPECT_ANY_THROW((void)(now != later)); + EXPECT_ANY_THROW((void)(now <= later)); + EXPECT_ANY_THROW((void)(now >= later)); + EXPECT_ANY_THROW((void)(now < later)); + EXPECT_ANY_THROW((void)(now > later)); + EXPECT_ANY_THROW((void)(now - later)); + + for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) { + rclcpp::Time time = rclcpp::Time(0, 0, time_source); + rclcpp::Time copy_constructor_time(time); + rclcpp::Time assignment_op_time = rclcpp::Time(1, 0, time_source); + assignment_op_time = time; + + EXPECT_TRUE(time == copy_constructor_time); + EXPECT_TRUE(time == assignment_op_time); + } +} + +TEST(TestTime, overflow_detectors) { + ///////////////////////////////////////////////////////////////////////////// + // Test logical_eq call first: + EXPECT_TRUE(logical_eq(false, false)); + EXPECT_FALSE(logical_eq(false, true)); + EXPECT_FALSE(logical_eq(true, false)); + EXPECT_TRUE(logical_eq(true, true)); + + ///////////////////////////////////////////////////////////////////////////// + // Exhaustive test of all int8_t values + using test_type_t = int8_t; + // big_type_t encompasses test_type_t: + // big_type_t::min < test_type_t::min + // big_type_t::max > test_type_t::max + using big_type_t = int16_t; + const big_type_t min_val = std::numeric_limits::min(); + const big_type_t max_val = std::numeric_limits::max(); + // 256 * 256 = 64K total loops, should be pretty fast on everything + for (big_type_t y = min_val; y <= max_val; ++y) { + for (big_type_t x = min_val; x <= max_val; ++x) { + const big_type_t sum = x + y; + const big_type_t diff = x - y; + + const bool add_will_overflow = + rclcpp::add_will_overflow(test_type_t(x), test_type_t(y)); + const bool add_did_overflow = sum > max_val; + EXPECT_TRUE(logical_eq(add_will_overflow, add_did_overflow)); + + const bool add_will_underflow = + rclcpp::add_will_underflow(test_type_t(x), test_type_t(y)); + const bool add_did_underflow = sum < min_val; + EXPECT_TRUE(logical_eq(add_will_underflow, add_did_underflow)); + + const bool sub_will_overflow = + rclcpp::sub_will_overflow(test_type_t(x), test_type_t(y)); + const bool sub_did_overflow = diff > max_val; + EXPECT_TRUE(logical_eq(sub_will_overflow, sub_did_overflow)); + + const bool sub_will_underflow = + rclcpp::sub_will_underflow(test_type_t(x), test_type_t(y)); + const bool sub_did_underflow = diff < min_val; + EXPECT_TRUE(logical_eq(sub_will_underflow, sub_did_underflow)); + } + } + + // Few selected tests for int64_t + EXPECT_TRUE(rclcpp::add_will_overflow(INT64_MAX, 1)); + EXPECT_FALSE(rclcpp::add_will_overflow(INT64_MAX, -1)); + EXPECT_TRUE(rclcpp::add_will_underflow(INT64_MIN, -1)); + EXPECT_FALSE(rclcpp::add_will_underflow(INT64_MIN, 1)); + + EXPECT_FALSE(rclcpp::sub_will_overflow(INT64_MAX, 1)); + EXPECT_TRUE(rclcpp::sub_will_overflow(INT64_MAX, -1)); + EXPECT_FALSE(rclcpp::sub_will_underflow(INT64_MIN, -1)); + EXPECT_TRUE(rclcpp::sub_will_underflow(INT64_MIN, 1)); +} + +TEST(TestTime, overflows) { + rclcpp::Time max_time(std::numeric_limits::max()); + rclcpp::Time min_time(std::numeric_limits::min()); + rclcpp::Duration one(1); + rclcpp::Duration two(2); + + // Cross min/max + EXPECT_THROW(max_time + one, std::overflow_error); + EXPECT_THROW(min_time - one, std::underflow_error); + EXPECT_THROW(max_time - min_time, std::overflow_error); + EXPECT_THROW(min_time - max_time, std::underflow_error); + EXPECT_NO_THROW(max_time - max_time); + EXPECT_NO_THROW(min_time - min_time); + + // Cross zero in both directions + rclcpp::Time one_time(1); + EXPECT_NO_THROW(one_time - two); + + rclcpp::Time minus_one_time(-1); + EXPECT_NO_THROW(minus_one_time + two); + + EXPECT_NO_THROW(one_time - minus_one_time); + EXPECT_NO_THROW(minus_one_time - one_time); + + rclcpp::Time two_time(2); + EXPECT_NO_THROW(one_time - two_time); +} + +TEST(TestTime, seconds) { + EXPECT_DOUBLE_EQ(0.0, rclcpp::Time(0, 0).seconds()); + EXPECT_DOUBLE_EQ(4.5, rclcpp::Time(4, 500000000).seconds()); + EXPECT_DOUBLE_EQ(2.5, rclcpp::Time(0, 2500000000).seconds()); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_time_source.cpp b/rclcpp-eloquent/rclcpp/test/test_time_source.cpp new file mode 100644 index 0000000..ef624bd --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_time_source.cpp @@ -0,0 +1,515 @@ +// Copyright 2017 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 +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" + +using namespace std::chrono_literals; + +class TestTimeSource : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +void spin_until_time( + rclcpp::Clock::SharedPtr clock, + rclcpp::Node::SharedPtr node, + std::chrono::nanoseconds end_time, + bool expect_time_update) +{ + // Call spin_once on the node until either: + // 1) We see the ros_clock's simulated time change to the expected end_time + // -or- + // 2) 1 second has elapsed in the real world + // If 'expect_time_update' is True, and we timed out waiting for simulated time to + // update, we'll have the test fail + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto start = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() < (start + 1s)) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + + executor.spin_once(10ms); + + if (clock->now().nanoseconds() == end_time.count()) { + return; + } + } + + if (expect_time_update) { + // If we were expecting ROS clock->now to be updated and we didn't take the early return from + // the loop up above, that's a failure + ASSERT_TRUE(false) << "Timed out waiting for ROS time to update"; + } +} + +void spin_until_ros_time_updated( + rclcpp::Clock::SharedPtr clock, + rclcpp::Node::SharedPtr node, + rclcpp::ParameterValue value) +{ + // Similar to above: Call spin_once until we see the clock's ros_time_is_active method + // match the ParameterValue + // Unlike spin_until_time, there aren't any test cases where we don't expect the value to + // update. In the event that the ParameterValue is not set, we'll pump messages for a full second + // but we don't cause the test to fail + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto start = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() < (start + 1s)) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + + executor.spin_once(10ms); + + // In the case where we didn't intend to change the parameter, we'll still pump + if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + continue; + } + + if (clock->ros_time_is_active() == value.get()) { + return; + } + } +} + +void trigger_clock_changes( + rclcpp::Node::SharedPtr node, + std::shared_ptr clock, + bool expect_time_update = true) +{ + auto clock_pub = node->create_publisher("clock", 10); + + for (int i = 0; i < 5; ++i) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + rosgraph_msgs::msg::Clock msg; + msg.clock.sec = i; + msg.clock.nanosec = 1000; + clock_pub->publish(msg); + + // workaround. Long-term, there can be a more elegant fix where we hook a future up + // to a clock change callback and spin until future complete, but that's an upstream + // change + spin_until_time( + clock, + node, + std::chrono::seconds(i) + std::chrono::nanoseconds(1000), + expect_time_update + ); + } +} + +void set_use_sim_time_parameter( + rclcpp::Node::SharedPtr node, + rclcpp::ParameterValue value, + rclcpp::Clock::SharedPtr clock) +{ + auto parameters_client = std::make_shared(node); + + using namespace std::chrono_literals; + EXPECT_TRUE(parameters_client->wait_for_service(2s)); + auto set_parameters_results = parameters_client->set_parameters( + { + rclcpp::Parameter("use_sim_time", value) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + + // Same as above - workaround for a little bit of asynchronus behavior. The sim_time paramater + // is set synchronously, but the way the ros clock gets notified involves a pub/sub that happens + // AFTER the synchronous notification that the parameter was set. This may also get fixed + // upstream + spin_until_ros_time_updated(clock, node, value); +} + +TEST_F(TestTimeSource, detachUnattached) { + rclcpp::TimeSource ts; + + ASSERT_NO_THROW(ts.detachNode()); + + // Try multiple detach to see if error + ASSERT_NO_THROW(ts.detachNode()); +} + +TEST_F(TestTimeSource, reattach) { + rclcpp::TimeSource ts; + // Try reattach + ASSERT_NO_THROW(ts.attachNode(node)); + ASSERT_NO_THROW(ts.attachNode(node)); +} + +TEST_F(TestTimeSource, ROS_time_valid_attach_detach) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + EXPECT_FALSE(ros_clock->ros_time_is_active()); + ts.attachClock(ros_clock); + auto now = ros_clock->now(); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.detachNode(); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); +} + +TEST_F(TestTimeSource, ROS_time_valid_wall_time) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + auto ros_clock2 = std::make_shared(RCL_ROS_TIME); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock2); + EXPECT_FALSE(ros_clock2->ros_time_is_active()); +} + +TEST_F(TestTimeSource, ROS_time_valid_sim_time) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + auto ros_clock2 = std::make_shared(RCL_ROS_TIME); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + ts.attachNode(node); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock2); + EXPECT_TRUE(ros_clock2->ros_time_is_active()); +} + +TEST_F(TestTimeSource, ROS_invalid_sim_time) { + rclcpp::TimeSource ts; + ts.attachNode(node); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter("use_sim_time", "not boolean")).successful); +} + +TEST_F(TestTimeSource, clock) { + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock, false); + + // Even now that we've recieved a message, ROS time should still not be active since the + // parameter has not been explicitly set. + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock); + + auto t_out = ros_clock->now(); + + // Time from clock should now reflect what was published on the /clock topic. + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); +} + +class CallbackObject +{ +public: + int pre_callback_calls_ = 0; + int last_precallback_id_ = 0; + void pre_callback(int id) + { + last_precallback_id_ = id; + ++pre_callback_calls_; + } + + int post_callback_calls_ = 0; + int last_postcallback_id_ = 0; + rcl_time_jump_t last_timejump_; + void post_callback(const rcl_time_jump_t & jump, int id) + { + last_postcallback_id_ = id; last_timejump_ = jump; + ++post_callback_calls_; + } +}; + +TEST_F(TestTimeSource, callbacks) { + CallbackObject cbo; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 0; + jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.on_clock_change = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 1), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); + + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // Last arg below is 'expect_time_update' Since ros_time is not active yet, we don't expect + // the simulated time to be updated by trigger_clock_changes. The method will pump messages + // anyway, but won't fail the test when the simulated time doesn't update + trigger_clock_changes(node, ros_clock, false); + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + + // Callbacks will not be triggered since ROS time is not active. + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); + + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock); + + auto t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + + // Callbacks will now have been triggered since ROS time is active. + EXPECT_EQ(1, cbo.last_precallback_id_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + + // Change callbacks + rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 2), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), + jump_threshold); + + trigger_clock_changes(node, ros_clock); + + EXPECT_EQ(2, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); + + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + + // Register a callback handler with only pre_callback + rclcpp::JumpHandler::SharedPtr callback_handler3 = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 3), + std::function(), + jump_threshold); + + trigger_clock_changes(node, ros_clock); + EXPECT_EQ(3, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); + + // Register a callback handler with only post_callback + rclcpp::JumpHandler::SharedPtr callback_handler4 = ros_clock->create_jump_callback( + std::function(), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4), + jump_threshold); + + trigger_clock_changes(node, ros_clock); + EXPECT_EQ(3, cbo.last_precallback_id_); + EXPECT_EQ(4, cbo.last_postcallback_id_); +} + + +TEST_F(TestTimeSource, callback_handler_erasure) { + CallbackObject cbo; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 0; + jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.on_clock_change = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 1), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + // Second callback handler + rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 1), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + // Callbacks will not be triggered since ROS time is not active. + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); + + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock); + + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + + // Callbacks will now have been triggered since ROS time is active. + EXPECT_EQ(1, cbo.last_precallback_id_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + + auto t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + + // Requeue a pointer in a new position + callback_handler = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 2), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), + jump_threshold); + + // Remove the last callback in the vector + callback_handler2.reset(); + + trigger_clock_changes(node, ros_clock); + + EXPECT_EQ(2, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); + + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); +} + +TEST_F(TestTimeSource, parameter_activation) { + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time + // should not be affected by the presence of a clock publisher. + trigger_clock_changes(node, ros_clock, false); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); +} + +TEST_F(TestTimeSource, no_pre_jump_callback) { + CallbackObject cbo; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 0; + jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.on_clock_change = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( + nullptr, + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + ASSERT_EQ(0, cbo.last_precallback_id_); + ASSERT_EQ(0, cbo.last_postcallback_id_); + ts.attachClock(ros_clock); + + // Activate ROS time + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + ASSERT_TRUE(ros_clock->ros_time_is_active()); + + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.pre_callback_calls_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + EXPECT_EQ(1, cbo.post_callback_calls_); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_timer.cpp b/rclcpp-eloquent/rclcpp/test/test_timer.cpp new file mode 100644 index 0000000..2d9022e --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_timer.cpp @@ -0,0 +1,153 @@ +// Copyright 2019 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 +#include +#include +#include + +#include "rcl/timer.h" + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +/// Timer testing bring up and teardown +class TestTimer : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + executor = std::make_shared(); + + has_timer_run.store(false); + cancel_timer.store(false); + + test_node = std::make_shared("test_timer_node"); + + timer = test_node->create_wall_timer( + 100ms, + [this]() -> void + { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); + } + // prevent any tests running timer from blocking + this->executor->cancel(); + } + ); + + executor->add_node(test_node); + // don't start spinning, let the test dictate when + } + + void TearDown() override + { + timer.reset(); + test_node.reset(); + executor.reset(); + rclcpp::shutdown(); + } + + // set to true if the timer callback executed, false otherwise + std::atomic has_timer_run; + // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise + // cancel the executor (preventing any tests from blocking) + std::atomic cancel_timer; + rclcpp::Node::SharedPtr test_node; + std::shared_ptr timer; + std::shared_ptr executor; +}; + +/// check if initial states are set as expected +void test_initial_conditions( + std::shared_ptr & timer, + std::atomic & has_timer_run) +{ + ASSERT_FALSE(timer->is_canceled()); + ASSERT_FALSE(has_timer_run.load()); +} + +/// Simple test +TEST_F(TestTimer, test_simple_cancel) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // cancel + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); + + EXPECT_FALSE(has_timer_run.load()); +} + +/// Test state when using reset +TEST_F(TestTimer, test_is_canceled_reset) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // reset shouldn't affect state (not canceled yet) + timer->reset(); + EXPECT_FALSE(timer->is_canceled()); + + // cancel after reset + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); + + // reset and cancel + timer->reset(); + EXPECT_FALSE(timer->is_canceled()); + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); + + EXPECT_FALSE(has_timer_run.load()); +} + +/// Run and check state, cancel the executor +TEST_F(TestTimer, test_run_cancel_executor) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // run the timer (once, this forces an executor cancel so spin won't block) + // but the timer was not explicitly cancelled + executor->spin(); + EXPECT_TRUE(has_timer_run.load()); + + // force a timer cancel + EXPECT_FALSE(timer->is_canceled()); + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); +} + +/// Run and check state, cancel the timer +TEST_F(TestTimer, test_run_cancel_timer) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // force a timer cancellation + cancel_timer.store(true); + // run the timer (once, this forces an executor cancel so spin won't block) + executor->spin(); + EXPECT_TRUE(has_timer_run.load()); + EXPECT_TRUE(timer->is_canceled()); +} diff --git a/rclcpp-eloquent/rclcpp/test/test_utilities.cpp b/rclcpp-eloquent/rclcpp/test/test_utilities.cpp new file mode 100644 index 0000000..8dd8d58 --- /dev/null +++ b/rclcpp-eloquent/rclcpp/test/test_utilities.cpp @@ -0,0 +1,80 @@ +// Copyright 2017 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 +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/utilities.hpp" + +TEST(TestUtilities, remove_ros_arguments) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + auto args = rclcpp::remove_ros_arguments(argc, argv); + + ASSERT_EQ(4u, args.size()); + ASSERT_EQ(std::string{"process_name"}, args[0]); + ASSERT_EQ(std::string{"-d"}, args[1]); + ASSERT_EQ(std::string{"--foo=bar"}, args[2]); + ASSERT_EQ(std::string{"--baz"}, args[3]); +} + +TEST(TestUtilities, init_with_args) { + const char * const argv[] = {"process_name"}; + int argc = sizeof(argv) / sizeof(const char *); + auto other_args = rclcpp::init_and_remove_ros_arguments(argc, argv); + + ASSERT_EQ(1u, other_args.size()); + ASSERT_EQ(std::string{"process_name"}, other_args[0]); + + EXPECT_TRUE(rclcpp::ok()); + rclcpp::shutdown(); +} + +TEST(TestUtilities, multi_init) { + auto context1 = std::make_shared(); + auto context2 = std::make_shared(); + + EXPECT_FALSE(rclcpp::ok(context1)); + EXPECT_FALSE(rclcpp::ok(context2)); + + context1->init(0, nullptr); + + EXPECT_TRUE(rclcpp::ok(context1)); + EXPECT_FALSE(rclcpp::ok(context2)); + + context2->init(0, nullptr); + + EXPECT_TRUE(rclcpp::ok(context1)); + EXPECT_TRUE(rclcpp::ok(context2)); + + rclcpp::shutdown(context1); + + EXPECT_FALSE(rclcpp::ok(context1)); + EXPECT_TRUE(rclcpp::ok(context2)); + + rclcpp::shutdown(context2); + + EXPECT_FALSE(rclcpp::ok(context1)); + EXPECT_FALSE(rclcpp::ok(context2)); +} diff --git a/rclcpp-eloquent/rclcpp_action/CHANGELOG.rst b/rclcpp-eloquent/rclcpp_action/CHANGELOG.rst new file mode 100644 index 0000000..58ce89d --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/CHANGELOG.rst @@ -0,0 +1,79 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp_action +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + +0.8.5 (2020-12-04) +------------------ + +0.8.4 (2020-01-17) +------------------ + +0.8.3 (2019-11-19) +------------------ +* issue-919 Fixed "memory leak" in action clients (`#920 `_) +* Contributors: astere-cpr + +0.8.2 (2019-11-18) +------------------ +* Increased a timeout for the ``test_client`` tests. (`#917 `_) +* Contributors: Michel Hidalgo + +0.8.1 (2019-10-23) +------------------ +* Template node type for rclcpp action server and clients (`#892 `_) +* Trait tests for generated actions (`#853 `_) +* Do not throw exception in action client if take fails (`#888 `_) +* Contributors: Jacob Perron, Michael Carroll, Steven Macenski + +0.8.0 (2019-09-26) +------------------ +* Fix UnknownGoalHandle error string. (`#856 `_) +* Guard against making multiple result requests for a goal handle (`#808 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* Fix typo in test fixture tear down method name (`#787 `_) +* Contributors: Chris Lalancette, Dan Rose, Jacob Perron + +0.7.5 (2019-05-30) +------------------ + +0.7.4 (2019-05-29) +------------------ +* Guard against calling null goal response callback (`#738 `_) +* Contributors: Jacob Perron + +0.7.3 (2019-05-20) +------------------ + +0.7.2 (2019-05-08) +------------------ +* Added return code to CancelGoal service response. (`#710 `_) +* Contributors: Jacob Perron, William Woodall + +0.7.1 (2019-04-26) +------------------ +* Added optional callbacks to action client for goal, response, and result. (`#701 `_) +* Added overload for node interfaces. (`#700 `_) +* Renamed action state transitions. (`#677 `_) +* Contributors: Jacob Perron, Karsten Knese + +0.7.0 (2019-04-14) +------------------ +* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 `_) +* Added documentation to rclcpp_action. (`#650 `_) +* Updated to use separated action types. (`#601 `_) +* Updated to wait for action server before sending goal. (`#637 `_) +* Refactored server goal handle's try_canceling() function. (`#603 `_) +* Contributors: Emerson Knapp, Jacob Perron, Michel Hidalgo, Shane Loretz + +0.6.2 (2018-12-13) +------------------ + +0.6.1 (2018-12-07) +------------------ +* Added wait_for_action_server() for action clients (`#598 `_) +* Updated to adapt to action implicit changes (`#602 `_) +* Added rclcpp_action Server implementation (`#593 `_) +* Added action client implementation (`#594 `_) +* Added skeleton for Action Server and Client (`#579 `_) +* Contributors: Michel Hidalgo, Shane Loretz, William Woodall diff --git a/rclcpp-eloquent/rclcpp_action/CMakeLists.txt b/rclcpp-eloquent/rclcpp_action/CMakeLists.txt new file mode 100644 index 0000000..6b84077 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp_action) + +find_package(ament_cmake_ros REQUIRED) +find_package(action_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcl_action REQUIRED) +find_package(rosidl_generator_cpp REQUIRED) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories(include) + +set(${PROJECT_NAME}_SRCS + src/client.cpp + src/qos.cpp + src/server.cpp + src/server_goal_handle.cpp + src/types.cpp +) + +add_library(${PROJECT_NAME} + ${${PROJECT_NAME}_SRCS}) + +ament_target_dependencies(${PROJECT_NAME} + "action_msgs" + "rcl_action" + "rclcpp" + "rosidl_generator_c" + "rosidl_generator_cpp") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} + PRIVATE "RCLCPP_ACTION_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include) + +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# specific order: dependents before dependencies +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_export_dependencies(ament_cmake) +ament_export_dependencies(action_msgs) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rcl_action) +ament_export_dependencies(rosidl_generator_c) +ament_export_dependencies(rosidl_generator_cpp) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120) + if(TARGET test_client) + ament_target_dependencies(test_client + "test_msgs" + ) + target_link_libraries(test_client + ${PROJECT_NAME} + ) + endif() + + ament_add_gtest(test_server test/test_server.cpp) + if(TARGET test_server) + ament_target_dependencies(test_server + "test_msgs" + ) + target_link_libraries(test_server + ${PROJECT_NAME} + ) + endif() + + ament_add_gtest(test_traits test/test_traits.cpp) + if(TARGET test_traits) + ament_target_dependencies(test_traits + "test_msgs" + ) + target_link_libraries(test_traits + ${PROJECT_NAME} + ) + endif() +endif() + +ament_package() diff --git a/rclcpp-eloquent/rclcpp_action/Doxyfile b/rclcpp-eloquent/rclcpp_action/Doxyfile new file mode 100644 index 0000000..4b97902 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/Doxyfile @@ -0,0 +1,35 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_action" +PROJECT_NUMBER = master +PROJECT_BRIEF = "C++ ROS Action Client Library" + +# Use these lines to include the generated logging.hpp (update install path if needed) +#INPUT = ../../../../install_isolated/rclcpp/include +#STRIP_FROM_PATH = /Users/william/ros2_ws/install_isolated/rclcpp/include +# Otherwise just generate for the local (non-generated header files) +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +TAGFILES += "../../../../doxygen_tag_files/rcl_action.tag=http://docs.ros2.org/latest/api/rcl_action/" +TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +# Uncomment to generate tag files for cross-project linking. +# GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_action.tag" diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client.hpp new file mode 100644 index 0000000..d94508f --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client.hpp @@ -0,0 +1,647 @@ +// Copyright 2018 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 RCLCPP_ACTION__CLIENT_HPP_ +#define RCLCPP_ACTION__CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/types.hpp" +#include "rclcpp_action/visibility_control.hpp" + + +namespace rclcpp_action +{ +// Forward declaration +class ClientBaseImpl; + +/// Base Action Client implementation +/// \internal +/** + * This class should not be used directly by users wanting to create an aciton client. + * Instead users should use `rclcpp_action::Client<>`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ +class ClientBase : public rclcpp::Waitable +{ +public: + RCLCPP_ACTION_PUBLIC + virtual ~ClientBase(); + + /// Return true if there is an action server that is ready to take goal requests. + RCLCPP_ACTION_PUBLIC + bool + action_server_is_ready() const; + + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + template + bool + wait_for_action_server( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return wait_for_action_server_nanoseconds( + std::chrono::duration_cast(timeout) + ); + } + + // ------------- + // Waitables API + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_subscriptions() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_timers() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_clients() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_services() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute() override; + + // End Waitables API + // ----------------- + +protected: + RCLCPP_ACTION_PUBLIC + ClientBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & options); + + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + RCLCPP_ACTION_PUBLIC + bool + wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout); + + // ----------------------------------------------------- + // API for communication between ClientBase and Client<> + using ResponseCallback = std::function response)>; + + /// \internal + RCLCPP_ACTION_PUBLIC + rclcpp::Logger get_logger(); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + GoalUUID + generate_goal_id(); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + send_goal_request( + std::shared_ptr request, + ResponseCallback callback); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + send_result_request( + std::shared_ptr request, + ResponseCallback callback); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + send_cancel_request( + std::shared_ptr request, + ResponseCallback callback); + + /// \internal + virtual + std::shared_ptr + create_goal_response() const = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + handle_goal_response( + const rmw_request_id_t & response_header, + std::shared_ptr goal_response); + + /// \internal + virtual + std::shared_ptr + create_result_response() const = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + handle_result_response( + const rmw_request_id_t & response_header, + std::shared_ptr result_response); + + /// \internal + virtual + std::shared_ptr + create_cancel_response() const = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + handle_cancel_response( + const rmw_request_id_t & response_header, + std::shared_ptr cancel_response); + + /// \internal + virtual + std::shared_ptr + create_feedback_message() const = 0; + + /// \internal + virtual + void + handle_feedback_message(std::shared_ptr message) = 0; + + /// \internal + virtual + std::shared_ptr + create_status_message() const = 0; + + /// \internal + virtual + void + handle_status_message(std::shared_ptr message) = 0; + + // End API for communication between ClientBase and Client<> + // --------------------------------------------------------- + +private: + std::unique_ptr pimpl_; +}; + +/// Action Client +/** + * This class creates an action client. + * + * To create an instance of an action client use `rclcpp_action::create_client()`. + * + * Internally, this class is responsible for: + * - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and + * - calling user callbacks. + */ +template +class Client : public ClientBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) + + using Goal = typename ActionT::Goal; + using Feedback = typename ActionT::Feedback; + using GoalHandle = ClientGoalHandle; + using WrappedResult = typename GoalHandle::WrappedResult; + using GoalResponseCallback = + std::function)>; + using FeedbackCallback = typename GoalHandle::FeedbackCallback; + using ResultCallback = typename GoalHandle::ResultCallback; + using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; + using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; + using CancelCallback = std::function; + + /// Options for sending a goal. + /** + * This struct is used to pass parameters to the function `async_send_goal`. + */ + struct SendGoalOptions + { + SendGoalOptions() + : goal_response_callback(nullptr), + feedback_callback(nullptr), + result_callback(nullptr) + { + } + + /// Function called when the goal is accepted or rejected. + /** + * Takes a single argument that is a future to a goal handle shared pointer. + * If the goal is accepted, then the pointer points to a valid goal handle. + * If the goal is rejected, then pointer has the value `nullptr`. + * If an error occurs while waiting for the goal response an exception will be thrown + * when calling `future::get()`. + * Possible exceptions include `rclcpp::RCLError` and `rclcpp::RCLBadAlloc`. + */ + GoalResponseCallback goal_response_callback; + + /// Function called whenever feedback is received for the goal. + FeedbackCallback feedback_callback; + + /// Function called when the result for the goal is received. + ResultCallback result_callback; + }; + + /// Construct an action client. + /** + * This constructs an action client, but it will not work until it has been added to a node. + * Use `rclcpp_action::create_client()` to both construct and add to a node. + * + * \param[in] node_base A pointer to the base interface of a node. + * \param[in] node_graph A pointer to an interface that allows getting graph information about + * a node. + * \param[in] node_logging A pointer to an interface that allows getting a node's logger. + * \param[in] action_name The action name. + * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`. + */ + Client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rcl_action_client_options_t client_options = rcl_action_client_get_default_options() + ) + : ClientBase( + node_base, node_graph, node_logging, action_name, + rosidl_typesupport_cpp::get_action_type_support_handle(), + client_options) + { + } + + /// Send an action goal and asynchronously get the result. + /** + * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`. + * If the goal is rejected by an action server, then the future is set to a `nullptr`. + * + * The goal handle is used to monitor the status of the goal and get the final result. + * + * \param[in] goal The goal request. + * \param[in] options Options for sending the goal request. Contains references to callbacks for + * the goal response (accepted/rejected), feedback, and the final result. + * \return A future that completes when the goal has been accepted or rejected. + * If the goal is rejected, then the result will be a `nullptr`. + */ + std::shared_future + async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions()) + { + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); + using GoalRequest = typename ActionT::Impl::SendGoalService::Request; + auto goal_request = std::make_shared(); + goal_request->goal_id.uuid = this->generate_goal_id(); + goal_request->goal = goal; + this->send_goal_request( + std::static_pointer_cast(goal_request), + [this, goal_request, options, promise, future](std::shared_ptr response) mutable + { + using GoalResponse = typename ActionT::Impl::SendGoalService::Response; + auto goal_response = std::static_pointer_cast(response); + if (!goal_response->accepted) { + promise->set_value(nullptr); + if (options.goal_response_callback) { + options.goal_response_callback(future); + } + return; + } + GoalInfo goal_info; + goal_info.goal_id.uuid = goal_request->goal_id.uuid; + goal_info.stamp = goal_response->stamp; + // Do not use std::make_shared as friendship cannot be forwarded. + std::shared_ptr goal_handle( + new GoalHandle(goal_info, options.feedback_callback, options.result_callback)); + { + std::lock_guard guard(goal_handles_mutex_); + goal_handles_[goal_handle->get_goal_id()] = goal_handle; + } + promise->set_value(goal_handle); + if (options.goal_response_callback) { + options.goal_response_callback(future); + } + + if (options.result_callback) { + try { + this->make_result_aware(goal_handle); + } catch (...) { + promise->set_exception(std::current_exception()); + return; + } + } + }); + return future; + } + + /// Asynchronously get the result for an active goal. + /** + * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal + * state. + * \param[in] goal_handle The goal handle for which to get the result. + * \param[in] result_callback Optional callback that is called when the result is received. + * \return A future that is set to the goal result when the goal is finished. + */ + std::shared_future + async_get_result( + typename GoalHandle::SharedPtr goal_handle, + ResultCallback result_callback = nullptr) + { + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(); + } + if (result_callback) { + // This will override any previously registered callback + goal_handle->set_result_callback(result_callback); + } + this->make_result_aware(goal_handle); + return goal_handle->async_result(); + } + + /// Asynchronously request a goal be canceled. + /** + * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a + * terminal state. + * \param[in] goal_handle The goal handle requesting to be canceled. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_goal( + typename GoalHandle::SharedPtr goal_handle, + CancelCallback cancel_callback = nullptr) + { + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(); + } + auto cancel_request = std::make_shared(); + // cancel_request->goal_info.goal_id = goal_handle->get_goal_id(); + cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id(); + return async_cancel(cancel_request, cancel_callback); + } + + /// Asynchronously request for all goals to be canceled. + /** + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_all_goals(CancelCallback cancel_callback = nullptr) + { + auto cancel_request = std::make_shared(); + // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + std::fill( + cancel_request->goal_info.goal_id.uuid.begin(), + cancel_request->goal_info.goal_id.uuid.end(), 0u); + return async_cancel(cancel_request, cancel_callback); + } + + /// Asynchronously request all goals at or before a specified time be canceled. + /** + * \param[in] stamp The timestamp for the cancel goal request. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_goals_before( + const rclcpp::Time & stamp, + CancelCallback cancel_callback = nullptr) + { + auto cancel_request = std::make_shared(); + // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + std::fill( + cancel_request->goal_info.goal_id.uuid.begin(), + cancel_request->goal_info.goal_id.uuid.end(), 0u); + cancel_request->goal_info.stamp = stamp; + return async_cancel(cancel_request, cancel_callback); + } + + virtual + ~Client() + { + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.begin(); + while (it != goal_handles_.end()) { + it->second->invalidate(); + it = goal_handles_.erase(it); + } + } + +private: + /// \internal + std::shared_ptr + create_goal_response() const override + { + using GoalResponse = typename ActionT::Impl::SendGoalService::Response; + return std::shared_ptr(new GoalResponse()); + } + + /// \internal + std::shared_ptr + create_result_response() const override + { + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; + return std::shared_ptr(new GoalResultResponse()); + } + + /// \internal + std::shared_ptr + create_cancel_response() const override + { + return std::shared_ptr(new CancelResponse()); + } + + /// \internal + std::shared_ptr + create_feedback_message() const override + { + using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; + return std::shared_ptr(new FeedbackMessage()); + } + + /// \internal + void + handle_feedback_message(std::shared_ptr message) override + { + std::lock_guard guard(goal_handles_mutex_); + using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; + typename FeedbackMessage::SharedPtr feedback_message = + std::static_pointer_cast(message); + const GoalUUID & goal_id = feedback_message->goal_id.uuid; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received feedback for unknown goal. Ignoring..."); + return; + } + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + auto feedback = std::make_shared(); + *feedback = feedback_message->feedback; + goal_handle->call_feedback_callback(goal_handle, feedback); + } + + /// \internal + std::shared_ptr + create_status_message() const override + { + using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; + return std::shared_ptr(new GoalStatusMessage()); + } + + /// \internal + void + handle_status_message(std::shared_ptr message) override + { + std::lock_guard guard(goal_handles_mutex_); + using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; + auto status_message = std::static_pointer_cast(message); + for (const GoalStatus & status : status_message->status_list) { + const GoalUUID & goal_id = status.goal_info.goal_id.uuid; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received status for unknown goal. Ignoring..."); + continue; + } + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + goal_handle->set_status(status.status); + const int8_t goal_status = goal_handle->get_status(); + if ( + goal_status == GoalStatus::STATUS_SUCCEEDED || + goal_status == GoalStatus::STATUS_CANCELED || + goal_status == GoalStatus::STATUS_ABORTED) + { + goal_handles_.erase(goal_id); + } + } + } + + /// \internal + void + make_result_aware(typename GoalHandle::SharedPtr goal_handle) + { + // Avoid making more than one request + if (goal_handle->set_result_awareness(true)) { + return; + } + using GoalResultRequest = typename ActionT::Impl::GetResultService::Request; + auto goal_result_request = std::make_shared(); + goal_result_request->goal_id.uuid = goal_handle->get_goal_id(); + this->send_result_request( + std::static_pointer_cast(goal_result_request), + [goal_handle, this](std::shared_ptr response) mutable + { + // Wrap the response in a struct with the fields a user cares about + WrappedResult wrapped_result; + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; + auto result_response = std::static_pointer_cast(response); + wrapped_result.result = std::make_shared(); + *wrapped_result.result = result_response->result; + wrapped_result.goal_id = goal_handle->get_goal_id(); + wrapped_result.code = static_cast(result_response->status); + goal_handle->set_result(wrapped_result); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + }); + } + + /// \internal + std::shared_future + async_cancel( + typename CancelRequest::SharedPtr cancel_request, + CancelCallback cancel_callback = nullptr) + { + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); + this->send_cancel_request( + std::static_pointer_cast(cancel_request), + [cancel_callback, promise](std::shared_ptr response) mutable + { + auto cancel_response = std::static_pointer_cast(response); + promise->set_value(cancel_response); + if (cancel_callback) { + cancel_callback(cancel_response); + } + }); + return future; + } + + std::map goal_handles_; + std::mutex goal_handles_mutex_; +}; +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CLIENT_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp new file mode 100644 index 0000000..42e2844 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -0,0 +1,165 @@ +// Copyright 2018 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 RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ +#define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "rclcpp_action/types.hpp" +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ +/// The possible statuses that an action goal can finish with. +enum class ResultCode : int8_t +{ + UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN, + SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, + CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED, + ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED +}; + + +// Forward declarations +template +class Client; + +/// Class for interacting with goals sent from action clients. +/** + * Use this class to check the status of a goal as well as get the result. + * + * This class is not meant to be created by a user, instead it is created when a goal has been + * accepted. + * A `Client` will create an instance and return it to the user (via a future) after calling + * `Client::async_send_goal`. + */ +template +class ClientGoalHandle +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) + + // A wrapper that defines the result of an action + typedef struct WrappedResult + { + /// The unique identifier of the goal + GoalUUID goal_id; + /// A status to indicate if the goal was canceled, aborted, or suceeded + ResultCode code; + /// User defined fields sent back with an action + typename ActionT::Result::SharedPtr result; + } WrappedResult; + + using Feedback = typename ActionT::Feedback; + using Result = typename ActionT::Result; + using FeedbackCallback = + std::function::SharedPtr, + const std::shared_ptr)>; + using ResultCallback = std::function; + + virtual ~ClientGoalHandle(); + + /// Get the unique ID for the goal. + const GoalUUID & + get_goal_id() const; + + /// Get the time when the goal was accepted. + rclcpp::Time + get_goal_stamp() const; + + /// Get a future to the goal result. + /** + * This method should not be called if the `ignore_result` flag was set when + * sending the original goal request (see Client::async_send_goal). + * + * `is_result_aware()` can be used to check if it is safe to call this method. + * + * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result. + * \return A future to the result. + */ + std::shared_future + async_result(); + + /// Get the goal status code. + int8_t + get_status(); + + /// Check if an action client has subscribed to feedback for the goal. + bool + is_feedback_aware(); + + /// Check if an action client has requested the result for the goal. + bool + is_result_aware(); + +private: + // The templated Client creates goal handles + friend Client; + + ClientGoalHandle( + const GoalInfo & info, + FeedbackCallback feedback_callback, + ResultCallback result_callback); + + void + set_feedback_callback(FeedbackCallback callback); + + void + set_result_callback(ResultCallback callback); + + void + call_feedback_callback( + typename ClientGoalHandle::SharedPtr shared_this, + typename std::shared_ptr feedback_message); + + /// Returns the previous value of awareness + bool + set_result_awareness(bool awareness); + + void + set_status(int8_t status); + + void + set_result(const WrappedResult & wrapped_result); + + void + invalidate(); + + GoalInfo info_; + + bool is_result_aware_{false}; + std::promise result_promise_; + std::shared_future result_future_; + + FeedbackCallback feedback_callback_{nullptr}; + ResultCallback result_callback_{nullptr}; + int8_t status_{GoalStatus::STATUS_ACCEPTED}; + + std::mutex handle_mutex_; +}; +} // namespace rclcpp_action + +#include // NOLINT(build/include_order) +#endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp new file mode 100644 index 0000000..402bd25 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -0,0 +1,169 @@ +// Copyright 2018 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 RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ +#define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ + +#include + +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/exceptions.hpp" + +namespace rclcpp_action +{ + +template +ClientGoalHandle::ClientGoalHandle( + const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback) +: info_(info), + result_future_(result_promise_.get_future()), + feedback_callback_(feedback_callback), + result_callback_(result_callback) +{ +} + +template +ClientGoalHandle::~ClientGoalHandle() +{ +} + +template +const GoalUUID & +ClientGoalHandle::get_goal_id() const +{ + return info_.goal_id.uuid; +} + +template +rclcpp::Time +ClientGoalHandle::get_goal_stamp() const +{ + return info_.stamp; +} + +template +std::shared_future::WrappedResult> +ClientGoalHandle::async_result() +{ + std::lock_guard guard(handle_mutex_); + if (!is_result_aware_) { + throw exceptions::UnawareGoalHandleError(); + } + return result_future_; +} + +template +void +ClientGoalHandle::set_result(const WrappedResult & wrapped_result) +{ + std::lock_guard guard(handle_mutex_); + status_ = static_cast(wrapped_result.code); + result_promise_.set_value(wrapped_result); + if (result_callback_) { + result_callback_(wrapped_result); + } +} + +template +void +ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) +{ + std::lock_guard guard(handle_mutex_); + feedback_callback_ = callback; +} + +template +void +ClientGoalHandle::set_result_callback(ResultCallback callback) +{ + std::lock_guard guard(handle_mutex_); + result_callback_ = callback; +} + +template +int8_t +ClientGoalHandle::get_status() +{ + std::lock_guard guard(handle_mutex_); + return status_; +} + +template +void +ClientGoalHandle::set_status(int8_t status) +{ + std::lock_guard guard(handle_mutex_); + status_ = status; +} + +template +bool +ClientGoalHandle::is_feedback_aware() +{ + std::lock_guard guard(handle_mutex_); + return feedback_callback_ != nullptr; +} + +template +bool +ClientGoalHandle::is_result_aware() +{ + std::lock_guard guard(handle_mutex_); + return is_result_aware_; +} + +template +bool +ClientGoalHandle::set_result_awareness(bool awareness) +{ + std::lock_guard guard(handle_mutex_); + bool previous = is_result_aware_; + is_result_aware_ = awareness; + return previous; +} + +template +void +ClientGoalHandle::invalidate() +{ + std::lock_guard guard(handle_mutex_); + status_ = GoalStatus::STATUS_UNKNOWN; + result_promise_.set_exception(std::make_exception_ptr(exceptions::UnawareGoalHandleError())); +} + +template +void +ClientGoalHandle::call_feedback_callback( + typename ClientGoalHandle::SharedPtr shared_this, + typename std::shared_ptr feedback_message) +{ + if (shared_this.get() != this) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle."); + return; + } + std::lock_guard guard(handle_mutex_); + if (nullptr == feedback_callback_) { + // Normal, some feedback messages may arrive after the goal result. + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it."); + return; + } + feedback_callback_(shared_this, feedback_message); +} + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_client.hpp new file mode 100644 index 0000000..6a356ce --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -0,0 +1,117 @@ +// Copyright 2018 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 RCLCPP_ACTION__CREATE_CLIENT_HPP_ +#define RCLCPP_ACTION__CREATE_CLIENT_HPP_ + +#include + +#include +#include + +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ +/// Create an action client. +/** + * This function is equivalent to \sa create_client()` however is using the individual + * node interfaces to create the client. + * + * \param node_base_interface[in] The node base interface of the corresponding node. + * \param node_graph_interface[in] The node graph interface of the corresponding node. + * \param node_logging_interface[in] The node logging interface of the corresponding node. + * \param node_waitables_interface[in] The node waitables interface of the corresponding node. + * \param[in] name The action name. + * \param[in] group The action client will be added to this callback group. + * If `nullptr`, then the action client is added to the default callback group. + */ +template +typename Client::SharedPtr +create_client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, + const std::string & name, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) +{ + std::weak_ptr weak_node = + node_waitables_interface; + std::weak_ptr weak_group = group; + bool group_is_null = (nullptr == group.get()); + + auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) + { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (!shared_node) { + return; + } + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specfic group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + delete ptr; + }; + + std::shared_ptr> action_client( + new Client( + node_base_interface, + node_graph_interface, + node_logging_interface, + name), + deleter); + + node_waitables_interface->add_waitable(action_client, group); + return action_client; +} + +/// Create an action client. +/** + * \param[in] node The action client will be added to this node. + * \param[in] name The action name. + * \param[in] group The action client will be added to this callback group. + * If `nullptr`, then the action client is added to the default callback group. + */ +template +typename Client::SharedPtr +create_client( + NodeT node, + const std::string & name, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) +{ + return create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + name, + group); +} +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CREATE_CLIENT_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_server.hpp new file mode 100644 index 0000000..afd24cd --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -0,0 +1,155 @@ +// Copyright 2018 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 RCLCPP_ACTION__CREATE_SERVER_HPP_ +#define RCLCPP_ACTION__CREATE_SERVER_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "rclcpp_action/server.hpp" +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ +/// Create an action server. +/** + * All provided callback functions must be non-blocking. + * This function is equivalent to \sa create_server()` however is using the individual + * node interfaces to create the server. + * + * \sa Server::Server() for more information. + * + * \param node_base_interface[in] The node base interface of the corresponding node. + * \param node_clock_interface[in] The node clock interface of the corresponding node. + * \param node_logging_interface[in] The node logging interface of the corresponding node. + * \param node_waitables_interface[in] The node waitables interface of the corresponding node. + * \param name[in] The action name. + * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. + * \param[in] options options to pass to the underlying `rcl_action_server_t`. + * \param group[in] The action server will be added to this callback group. + * If `nullptr`, then the action server is added to the default callback group. + */ +template +typename Server::SharedPtr +create_server( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, + const std::string & name, + typename Server::GoalCallback handle_goal, + typename Server::CancelCallback handle_cancel, + typename Server::AcceptedCallback handle_accepted, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) +{ + std::weak_ptr weak_node = + node_waitables_interface; + std::weak_ptr weak_group = group; + bool group_is_null = (nullptr == group.get()); + + auto deleter = [weak_node, weak_group, group_is_null](Server * ptr) + { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (!shared_node) { + return; + } + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specfic group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + delete ptr; + }; + + std::shared_ptr> action_server(new Server( + node_base_interface, + node_clock_interface, + node_logging_interface, + name, + options, + handle_goal, + handle_cancel, + handle_accepted), deleter); + + node_waitables_interface->add_waitable(action_server, group); + return action_server; +} + +/// Create an action server. +/** + * All provided callback functions must be non-blocking. + * + * \sa Server::Server() for more information. + * + * \param node[in] The action server will be added to this node. + * \param name[in] The action name. + * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. + * \param[in] options options to pass to the underlying `rcl_action_server_t`. + * \param group[in] The action server will be added to this callback group. + * If `nullptr`, then the action server is added to the default callback group. + */ +template +typename Server::SharedPtr +create_server( + NodeT node, + const std::string & name, + typename Server::GoalCallback handle_goal, + typename Server::CancelCallback handle_cancel, + typename Server::AcceptedCallback handle_accepted, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr) +{ + return create_server( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + name, + handle_goal, + handle_cancel, + handle_accepted, + options, + group); +} +} // namespace rclcpp_action +#endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/exceptions.hpp new file mode 100644 index 0000000..33d94c2 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -0,0 +1,46 @@ +// Copyright 2018 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 RCLCPP_ACTION__EXCEPTIONS_HPP_ +#define RCLCPP_ACTION__EXCEPTIONS_HPP_ + +#include + +namespace rclcpp_action +{ +namespace exceptions +{ +class UnknownGoalHandleError : public std::invalid_argument +{ +public: + UnknownGoalHandleError() + : std::invalid_argument("Goal handle is not known to this client.") + { + } +}; + +class UnawareGoalHandleError : public std::runtime_error +{ +public: + UnawareGoalHandleError() + : std::runtime_error("Goal handle is not tracking the goal result.") + { + } +}; + +} // namespace exceptions + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__EXCEPTIONS_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/qos.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/qos.hpp new file mode 100644 index 0000000..2acfddb --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/qos.hpp @@ -0,0 +1,34 @@ +// Copyright 2019 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 RCLCPP_ACTION__QOS_HPP_ +#define RCLCPP_ACTION__QOS_HPP_ + +#include + +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ + +class DefaultActionStatusQoS : public rclcpp::QoS +{ +public: + RCLCPP_ACTION_PUBLIC + DefaultActionStatusQoS(); +}; + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__QOS_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp new file mode 100644 index 0000000..61122e9 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp @@ -0,0 +1,44 @@ +// Copyright 2018 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. + +/** \mainpage rclcpp_action: ROS Action Client Library for C++ + * + * `rclcpp_action` provides the canonical C++ API for interacting with ROS Actions. + * It consists of these main components: + * + * - Action Client + * - rclcpp_action/client.hpp + * - rclcpp_action/create_client.hpp + * - rclcpp_action/client_goal_handle.hpp + * - Action Server + * - rclcpp_action/server.hpp + * - rclcpp_action/create_server.hpp + * - rclcpp_action/server_goal_handle.hpp + */ + +#ifndef RCLCPP_ACTION__RCLCPP_ACTION_HPP_ +#define RCLCPP_ACTION__RCLCPP_ACTION_HPP_ + +#include +#include + +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/create_client.hpp" +#include "rclcpp_action/create_server.hpp" +#include "rclcpp_action/server.hpp" +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp_action/visibility_control.hpp" + +#endif // RCLCPP_ACTION__RCLCPP_ACTION_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server.hpp new file mode 100644 index 0000000..7b618a0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server.hpp @@ -0,0 +1,486 @@ +// Copyright 2018 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 RCLCPP_ACTION__SERVER_HPP_ +#define RCLCPP_ACTION__SERVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp_action/visibility_control.hpp" +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp_action/types.hpp" + +namespace rclcpp_action +{ +// Forward declaration +class ServerBaseImpl; + +/// A response returned by an action server callback when a goal is requested. +enum class GoalResponse : int8_t +{ + /// The goal is rejected and will not be executed. + REJECT = 1, + /// The server accepts the goal, and is going to begin execution immediately. + ACCEPT_AND_EXECUTE = 2, + /// The server accepts the goal, and is going to execute it later. + ACCEPT_AND_DEFER = 3, +}; + +/// A response returned by an action server callback when a goal has been asked to be canceled. +enum class CancelResponse : int8_t +{ + /// The server will not try to cancel the goal. + REJECT = 1, + /// The server has agreed to try to cancel the goal. + ACCEPT = 2, +}; + +/// Base Action Server implementation +/// \internal +/** + * This class should not be used directly by users writing an action server. + * Instead users should use `rclcpp_action::Server`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ +class ServerBase : public rclcpp::Waitable +{ +public: + RCLCPP_ACTION_PUBLIC + virtual ~ServerBase(); + + // ------------- + // Waitables API + + /// Return the number of subscriptions used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_subscriptions() override; + + /// Return the number of timers used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_timers() override; + + /// Return the number of service clients used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_clients() override; + + /// Return the number of service servers used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_services() override; + + /// Return the number of guard conditions used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + + /// Add all entities to a wait set. + /// \internal + RCLCPP_ACTION_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + /// Return true if any entity belonging to the action server is ready to be executed. + /// \internal + RCLCPP_ACTION_PUBLIC + bool + is_ready(rcl_wait_set_t *) override; + + /// Act on entities in the wait set which are ready to be acted upon. + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute() override; + + // End Waitables API + // ----------------- + +protected: + RCLCPP_ACTION_PUBLIC + ServerBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & name, + const rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options); + + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + + // ServerBase will call this function when a goal request is received. + // The subclass should convert to the real type and call a user's callback. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::pair> + call_handle_goal_callback(GoalUUID &, std::shared_ptr request) = 0; + + // ServerBase will determine which goal ids are being cancelled, and then call this function for + // each goal id. + // The subclass should look up a goal handle and call the user's callback. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + CancelResponse + call_handle_cancel_callback(const GoalUUID & uuid) = 0; + + /// Given a goal request message, return the UUID contained within. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + GoalUUID + get_goal_id_from_goal_request(void * message) = 0; + + /// Create an empty goal request message so it can be taken from a lower layer. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::shared_ptr + create_goal_request() = 0; + + /// Call user callback to inform them a goal has been accepted. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + call_goal_accepted_callback( + std::shared_ptr rcl_goal_handle, + GoalUUID uuid, std::shared_ptr goal_request_message) = 0; + + /// Given a result request message, return the UUID contained within. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + GoalUUID + get_goal_id_from_result_request(void * message) = 0; + + /// Create an empty goal request message so it can be taken from a lower layer. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::shared_ptr + create_result_request() = 0; + + /// Create an empty goal result message so it can be sent as a reply in a lower layer + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::shared_ptr + create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_status(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + notify_goal_terminal_state(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_result(const GoalUUID & uuid, std::shared_ptr result_msg); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_feedback(std::shared_ptr feedback_msg); + + // End API for communication between ServerBase and Server<> + // --------------------------------------------------------- + +private: + /// Handle a request to add a new goal to the server + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_goal_request_received(); + + /// Handle a request to cancel goals on the server + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_cancel_request_received(); + + /// Handle a request to get the result of an action + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_result_request_received(); + + /// Handle a timeout indicating a completed goal should be forgotten by the server + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_check_expired_goals(); + + /// Private implementation + /// \internal + std::unique_ptr pimpl_; +}; + +/// Action Server +/** + * This class creates an action server. + * + * Create an instance of this server using `rclcpp_action::create_server()`. + * + * Internally, this class is responsible for: + * - coverting between the C++ action type and generic types for `rclcpp_action::ServerBase`, and + * - calling user callbacks. + */ +template +class Server : public ServerBase, public std::enable_shared_from_this> +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) + + /// Signature of a callback that accepts or rejects goal requests. + using GoalCallback = std::function)>; + /// Signature of a callback that accepts or rejects requests to cancel a goal. + using CancelCallback = std::function>)>; + /// Signature of a callback that is used to notify when the goal has been accepted. + using AcceptedCallback = std::function>)>; + + /// Construct an action server. + /** + * This constructs an action server, but it will not work until it has been added to a node. + * Use `rclcpp_action::create_server()` to both construct and add to a node. + * + * Three callbacks must be provided: + * - one to accept or reject goals sent to the server, + * - one to accept or reject requests to cancel a goal, + * - one to receive a goal handle after a goal has been accepted. + * All callbacks must be non-blocking. + * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle`. + * + * \param[in] node_base a pointer to the base interface of a node. + * \param[in] node_clock a pointer to an interface that allows getting a node's clock. + * \param[in] node_logging a pointer to an interface that allows getting a node's logger. + * \param[in] name the name of an action. + * The same name and type must be used by both the action client and action server to + * communicate. + * \param[in] options options to pass to the underlying `rcl_action_server_t`. + * \param[in] handle_goal a callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel a callback that decides if a goal should be attemted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_accepted a callback that is called to give the user a handle to the goal. + * execution. + */ + Server( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & name, + const rcl_action_server_options_t & options, + GoalCallback handle_goal, + CancelCallback handle_cancel, + AcceptedCallback handle_accepted + ) + : ServerBase( + node_base, + node_clock, + node_logging, + name, + rosidl_typesupport_cpp::get_action_type_support_handle(), + options), + handle_goal_(handle_goal), + handle_cancel_(handle_cancel), + handle_accepted_(handle_accepted) + { + } + + virtual ~Server() = default; + +protected: + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + + /// \internal + std::pair> + call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr message) override + { + auto request = std::static_pointer_cast< + typename ActionT::Impl::SendGoalService::Request>(message); + auto goal = std::shared_ptr(request, &request->goal); + GoalResponse user_response = handle_goal_(uuid, goal); + + auto ros_response = std::make_shared(); + ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || + GoalResponse::ACCEPT_AND_DEFER == user_response; + return std::make_pair(user_response, ros_response); + } + + /// \internal + CancelResponse + call_handle_cancel_callback(const GoalUUID & uuid) override + { + std::lock_guard lock(goal_handles_mutex_); + CancelResponse resp = CancelResponse::REJECT; + auto element = goal_handles_.find(uuid); + if (element != goal_handles_.end()) { + std::shared_ptr> goal_handle = element->second.lock(); + if (goal_handle) { + resp = handle_cancel_(goal_handle); + if (CancelResponse::ACCEPT == resp) { + goal_handle->_cancel_goal(); + } + } + } + return resp; + } + + /// \internal + void + call_goal_accepted_callback( + std::shared_ptr rcl_goal_handle, + GoalUUID uuid, std::shared_ptr goal_request_message) override + { + std::shared_ptr> goal_handle; + std::weak_ptr> weak_this = this->shared_from_this(); + + std::function)> on_terminal_state = + [weak_this](const GoalUUID & uuid, std::shared_ptr result_message) + { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + // Send result message to anyone that asked + shared_this->publish_result(uuid, result_message); + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + // notify base so it can recalculate the expired goal timer + shared_this->notify_goal_terminal_state(); + // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires) + std::lock_guard lock(shared_this->goal_handles_mutex_); + shared_this->goal_handles_.erase(uuid); + }; + + std::function on_executing = + [weak_this](const GoalUUID & uuid) + { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + (void)uuid; + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + }; + + std::function)> publish_feedback = + [weak_this](std::shared_ptr feedback_msg) + { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + shared_this->publish_feedback(std::static_pointer_cast(feedback_msg)); + }; + + auto request = std::static_pointer_cast< + const typename ActionT::Impl::SendGoalService::Request>(goal_request_message); + auto goal = std::shared_ptr(request, &request->goal); + goal_handle.reset( + new ServerGoalHandle( + rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback)); + { + std::lock_guard lock(goal_handles_mutex_); + goal_handles_[uuid] = goal_handle; + } + handle_accepted_(goal_handle); + } + + /// \internal + GoalUUID + get_goal_id_from_goal_request(void * message) override + { + return + static_cast(message)->goal_id.uuid; + } + + /// \internal + std::shared_ptr + create_goal_request() override + { + return std::shared_ptr(new typename ActionT::Impl::SendGoalService::Request()); + } + + /// \internal + GoalUUID + get_goal_id_from_result_request(void * message) override + { + return + static_cast(message)->goal_id.uuid; + } + + /// \internal + std::shared_ptr + create_result_request() override + { + return std::shared_ptr(new typename ActionT::Impl::GetResultService::Request()); + } + + /// \internal + std::shared_ptr + create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override + { + auto result = std::make_shared(); + result->status = status; + return std::static_pointer_cast(result); + } + + // End API for communication between ServerBase and Server<> + // --------------------------------------------------------- + +private: + GoalCallback handle_goal_; + CancelCallback handle_cancel_; + AcceptedCallback handle_accepted_; + + using GoalHandleWeakPtr = std::weak_ptr>; + /// A map of goal id to goal handle weak pointers. + /// This is used to provide a goal handle to handle_cancel. + std::unordered_map goal_handles_; + std::mutex goal_handles_mutex_; +}; +} // namespace rclcpp_action +#endif // RCLCPP_ACTION__SERVER_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp new file mode 100644 index 0000000..d41b32e --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -0,0 +1,284 @@ +// Copyright 2018 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 RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ +#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ + +#include +#include + +#include + +#include +#include +#include + +#include "rclcpp_action/visibility_control.hpp" +#include "rclcpp_action/types.hpp" + +namespace rclcpp_action +{ + +/// Base class to interact with goals on a server. +/// \internal +/** + * + * This class in not be used directly by users writing an action server. + * Instead users will be given an instance of `rclcpp_action::ServerGoalHandle<>`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ +class ServerGoalHandleBase +{ +public: + /// Indicate if client has requested this goal be cancelled. + /// \return true if a cancelation request has been accepted for this goal. + RCLCPP_ACTION_PUBLIC + bool + is_canceling() const; + + /// Indicate if goal is pending or executing. + /// \return false if goal has reached a terminal state. + RCLCPP_ACTION_PUBLIC + bool + is_active() const; + + /// Indicate if goal is executing. + /// \return true only if the goal is in an executing state. + RCLCPP_ACTION_PUBLIC + bool + is_executing() const; + + RCLCPP_ACTION_PUBLIC + virtual + ~ServerGoalHandleBase(); + +protected: + // ------------------------------------------------------------------------- + // API for communication between ServerGoalHandleBase and ServerGoalHandle<> + + /// \internal + RCLCPP_ACTION_PUBLIC + ServerGoalHandleBase( + std::shared_ptr rcl_handle + ) + : rcl_handle_(rcl_handle) + { + } + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _abort(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _succeed(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _cancel_goal(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _canceled(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _execute(); + + /// Transition the goal to canceled state if it never reached a terminal state. + /// \internal + RCLCPP_ACTION_PUBLIC + bool + try_canceling() noexcept; + + // End API for communication between ServerGoalHandleBase and ServerGoalHandle<> + // ----------------------------------------------------------------------------- + +private: + std::shared_ptr rcl_handle_; + mutable std::mutex rcl_handle_mutex_; +}; + +// Forward declare server +template +class Server; + +/// Class to interact with goals on a server. +/** + * Use this class to check the status of a goal as well as set the result. + * + * This class is not meant to be created by a user, instead it is created when a goal has been + * accepted. + * A `Server` will create an instance and give it to the user in their `handle_accepted` callback. + * + * Internally, this class is responsible for coverting between the C++ action type and generic + * types for `rclcpp_action::ServerGoalHandleBase`. + */ +template +class ServerGoalHandle : public ServerGoalHandleBase +{ +public: + /// Send an update about the progress of a goal. + /** + * This must only be called when the goal is executing. + * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called + * first. + * + * \throws std::runtime_error If the goal is in any state besides executing. + * + * \param[in] feedback_msg the message to publish to clients. + */ + void + publish_feedback(std::shared_ptr feedback_msg) + { + auto feedback_message = std::make_shared(); + feedback_message->goal_id.uuid = uuid_; + feedback_message->feedback = *feedback_msg; + publish_feedback_(feedback_message); + } + + /// Indicate that a goal could not be reached and has been aborted. + /** + * Only call this if the goal was executing but cannot be completed. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. + */ + void + abort(typename ActionT::Result::SharedPtr result_msg) + { + _abort(); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); + } + + /// Indicate that a goal has succeeded. + /** + * Only call this if the goal is executing and has reached the desired final state. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. + */ + void + succeed(typename ActionT::Result::SharedPtr result_msg) + { + _succeed(); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); + } + + /// Indicate that a goal has been canceled. + /** + * Only call this if the goal is executing or pending, but has been canceled. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. + */ + void + canceled(typename ActionT::Result::SharedPtr result_msg) + { + _canceled(); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); + } + + /// Indicate that the server is starting to execute a goal. + /** + * Only call this if the goal is pending. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + */ + void + execute() + { + _execute(); + on_executing_(uuid_); + } + + /// Get the user provided message describing the goal. + const std::shared_ptr + get_goal() const + { + return goal_; + } + + /// Get the unique identifier of the goal + const GoalUUID & + get_goal_id() const + { + return uuid_; + } + + virtual ~ServerGoalHandle() + { + // Cancel goal if handle was allowed to destruct without reaching a terminal state + if (try_canceling()) { + auto null_result = std::make_shared(); + null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + on_terminal_state_(uuid_, null_result); + } + } + +protected: + /// \internal + ServerGoalHandle( + std::shared_ptr rcl_handle, + GoalUUID uuid, + std::shared_ptr goal, + std::function)> on_terminal_state, + std::function on_executing, + std::function)> publish_feedback + ) + : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), + on_terminal_state_(on_terminal_state), on_executing_(on_executing), + publish_feedback_(publish_feedback) + { + } + + /// The user provided message describing the goal. + const std::shared_ptr goal_; + + /// A unique id for the goal request. + const GoalUUID uuid_; + + friend Server; + + std::function)> on_terminal_state_; + std::function on_executing_; + std::function)> publish_feedback_; +}; +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/types.hpp new file mode 100644 index 0000000..addf4a8 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/types.hpp @@ -0,0 +1,84 @@ +// Copyright 2018 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 RCLCPP_ACTION__TYPES_HPP_ +#define RCLCPP_ACTION__TYPES_HPP_ + +#include + +#include +#include + +#include +#include +#include + +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ + +using GoalUUID = std::array; +using GoalStatus = action_msgs::msg::GoalStatus; +using GoalInfo = action_msgs::msg::GoalInfo; + +/// Convert a goal id to a human readable string. +RCLCPP_ACTION_PUBLIC +std::string +to_string(const GoalUUID & goal_id); + +// Convert C++ GoalID to rcl_action_goal_info_t +RCLCPP_ACTION_PUBLIC +void +convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info); + +// Convert rcl_action_goal_info_t to C++ GoalID +RCLCPP_ACTION_PUBLIC +void +convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id); +} // namespace rclcpp_action + +namespace std +{ +template<> +struct less +{ + bool operator()( + const rclcpp_action::GoalUUID & lhs, + const rclcpp_action::GoalUUID & rhs) const + { + return lhs < rhs; + } +}; + +/// Hash a goal id so it can be used as a key in std::unordered_map +template<> +struct hash +{ + size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept + { + // TODO(sloretz) Use someone else's hash function and cite it + size_t result = 0; + for (size_t i = 0; i < uuid.size(); ++i) { + for (size_t b = 0; b < sizeof(size_t); ++b) { + size_t part = uuid[i]; + part <<= CHAR_BIT * b; + result ^= part; + } + } + return result; + } +}; +} // namespace std +#endif // RCLCPP_ACTION__TYPES_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/visibility_control.hpp b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/visibility_control.hpp new file mode 100644 index 0000000..32b03dd --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/include/rclcpp_action/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2018 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_ +#define RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCLCPP_ACTION_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_ACTION_IMPORT __attribute__ ((dllimport)) + #else + #define RCLCPP_ACTION_EXPORT __declspec(dllexport) + #define RCLCPP_ACTION_IMPORT __declspec(dllimport) + #endif + #ifdef RCLCPP_ACTION_BUILDING_LIBRARY + #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_EXPORT + #else + #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_IMPORT + #endif + #define RCLCPP_ACTION_PUBLIC_TYPE RCLCPP_ACTION_PUBLIC + #define RCLCPP_ACTION_LOCAL +#else + #define RCLCPP_ACTION_EXPORT __attribute__ ((visibility("default"))) + #define RCLCPP_ACTION_IMPORT + #if __GNUC__ >= 4 + #define RCLCPP_ACTION_PUBLIC __attribute__ ((visibility("default"))) + #define RCLCPP_ACTION_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCLCPP_ACTION_PUBLIC + #define RCLCPP_ACTION_LOCAL + #endif + #define RCLCPP_ACTION_PUBLIC_TYPE +#endif + +#endif // RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_ diff --git a/rclcpp-eloquent/rclcpp_action/package.xml b/rclcpp-eloquent/rclcpp_action/package.xml new file mode 100644 index 0000000..359d52c --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/package.xml @@ -0,0 +1,32 @@ + + + + rclcpp_action + 0.8.5 + Adds action APIs for C++. + Dirk Thomas + Apache License 2.0 + + ament_cmake_ros + + rosidl_generator_cpp + rosidl_generator_c + + rosidl_generator_c + rosidl_generator_cpp + + action_msgs + rclcpp + rcl_action + + ament_cmake + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + test_msgs + + + ament_cmake + + diff --git a/rclcpp-eloquent/rclcpp_action/src/client.cpp b/rclcpp-eloquent/rclcpp_action/src/client.cpp new file mode 100644 index 0000000..d15a7ae --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/src/client.cpp @@ -0,0 +1,444 @@ +// Copyright 2018 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 +#include +#include + +#include +#include +#include +#include +#include + +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/exceptions.hpp" + +namespace rclcpp_action +{ + +class ClientBaseImpl +{ +public: + ClientBaseImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & client_options) + : node_graph_(node_graph), + node_handle(node_base->get_shared_rcl_node_handle()), + logger(node_logging->get_logger().get_child("rclcpp_acton")), + random_bytes_generator(std::random_device{} ()) + { + std::weak_ptr weak_node_handle(node_handle); + client_handle = std::shared_ptr( + new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client) + { + auto handle = weak_node_handle.lock(); + if (handle) { + if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) { + RCLCPP_ERROR( + rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp_action"), + "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp_action"), + "Error in destruction of rcl action client handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete client; + }); + *client_handle = rcl_action_get_zero_initialized_client(); + rcl_ret_t ret = rcl_action_client_init( + client_handle.get(), node_handle.get(), type_support, + action_name.c_str(), &client_options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not initialize rcl action client"); + } + + ret = rcl_action_client_wait_set_get_num_entities( + client_handle.get(), + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not retrieve rcl action client details"); + } + } + + size_t num_subscriptions{0u}; + size_t num_guard_conditions{0u}; + size_t num_timers{0u}; + size_t num_clients{0u}; + size_t num_services{0u}; + + bool is_feedback_ready{false}; + bool is_status_ready{false}; + bool is_goal_response_ready{false}; + bool is_cancel_response_ready{false}; + bool is_result_response_ready{false}; + + rclcpp::Context::SharedPtr context_; + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; + std::shared_ptr client_handle{nullptr}; + std::shared_ptr node_handle{nullptr}; + rclcpp::Logger logger; + + using ResponseCallback = std::function response)>; + + std::map pending_goal_responses; + std::mutex goal_requests_mutex; + + std::map pending_result_responses; + std::mutex result_requests_mutex; + + std::map pending_cancel_responses; + std::mutex cancel_requests_mutex; + + std::independent_bits_engine< + std::default_random_engine, 8, unsigned int> random_bytes_generator; +}; + +ClientBase::ClientBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & client_options) +: pimpl_(new ClientBaseImpl( + node_base, node_graph, node_logging, action_name, type_support, client_options)) +{ +} + +ClientBase::~ClientBase() +{ +} + +bool +ClientBase::action_server_is_ready() const +{ + bool is_ready; + rcl_ret_t ret = rcl_action_server_is_available( + this->pimpl_->node_handle.get(), + this->pimpl_->client_handle.get(), + &is_ready); + if (RCL_RET_NODE_INVALID == ret) { + const rcl_node_t * node_handle = this->pimpl_->node_handle.get(); + if (node_handle && !rcl_context_is_valid(node_handle->context)) { + // context is shutdown, do a soft failure + return false; + } + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_action_server_is_available failed"); + } + return is_ready; +} + +bool +ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) +{ + auto start = std::chrono::steady_clock::now(); + // make an event to reuse, rather than create a new one each time + auto node_ptr = pimpl_->node_graph_.lock(); + if (!node_ptr) { + throw rclcpp::exceptions::InvalidNodeError(); + } + // check to see if the server is ready immediately + if (this->action_server_is_ready()) { + return true; + } + auto event = node_ptr->get_graph_event(); + if (timeout == std::chrono::nanoseconds(0)) { + // check was non-blocking, return immediately + return false; + } + // update the time even on the first loop to account for time spent in the first call + // to this->server_is_ready() + std::chrono::nanoseconds time_to_wait = + timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); + if (time_to_wait < std::chrono::nanoseconds(0)) { + // Do not allow the time_to_wait to become negative when timeout was originally positive. + // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. + time_to_wait = std::chrono::nanoseconds(0); + } + do { + if (!rclcpp::ok(this->pimpl_->context_)) { + return false; + } + // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation. + // A race condition means that graph changes for services becoming available may trigger the + // wait set to wake up, but then not be reported as ready immediately after the wake up + // (see https://github.com/ros2/rmw_connext/issues/201) + // If no other graph events occur, the wait set will not be triggered again until the timeout + // has been reached, despite the service being available, so we artificially limit the wait + // time to limit the delay. + node_ptr->wait_for_graph_change( + event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100)))); + // Because of the aforementioned race condition, we check if the service is ready even if the + // graph event wasn't triggered. + event->check_and_clear(); + if (this->action_server_is_ready()) { + return true; + } + // server is not ready, loop if there is time left + if (timeout > std::chrono::nanoseconds(0)) { + time_to_wait = timeout - (std::chrono::steady_clock::now() - start); + } + // if timeout is negative, time_to_wait will never reach zero + } while (time_to_wait > std::chrono::nanoseconds(0)); + return false; // timeout exceeded while waiting for the server to be ready +} + +rclcpp::Logger +ClientBase::get_logger() +{ + return pimpl_->logger; +} + +size_t +ClientBase::get_number_of_ready_subscriptions() +{ + return pimpl_->num_subscriptions; +} + +size_t +ClientBase::get_number_of_ready_guard_conditions() +{ + return pimpl_->num_guard_conditions; +} + +size_t +ClientBase::get_number_of_ready_timers() +{ + return pimpl_->num_timers; +} + +size_t +ClientBase::get_number_of_ready_clients() +{ + return pimpl_->num_clients; +} + +size_t +ClientBase::get_number_of_ready_services() +{ + return pimpl_->num_services; +} + +bool +ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + rcl_ret_t ret = rcl_action_wait_set_add_action_client( + wait_set, pimpl_->client_handle.get(), nullptr, nullptr); + return RCL_RET_OK == ret; +} + +bool +ClientBase::is_ready(rcl_wait_set_t * wait_set) +{ + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( + wait_set, pimpl_->client_handle.get(), + &pimpl_->is_feedback_ready, + &pimpl_->is_status_ready, + &pimpl_->is_goal_response_ready, + &pimpl_->is_cancel_response_ready, + &pimpl_->is_result_response_ready); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to check for any ready entities"); + } + return + pimpl_->is_feedback_ready || + pimpl_->is_status_ready || + pimpl_->is_goal_response_ready || + pimpl_->is_cancel_response_ready || + pimpl_->is_result_response_ready; +} + +void +ClientBase::handle_goal_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::lock_guard guard(pimpl_->goal_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_goal_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown goal response, ignoring..."); + return; + } + pimpl_->pending_goal_responses[sequence_number](response); + pimpl_->pending_goal_responses.erase(sequence_number); +} + +void +ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback callback) +{ + std::unique_lock guard(pimpl_->goal_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_goal_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); + } + assert(pimpl_->pending_goal_responses.count(sequence_number) == 0); + pimpl_->pending_goal_responses[sequence_number] = callback; +} + +void +ClientBase::handle_result_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::lock_guard guard(pimpl_->result_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_result_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring..."); + return; + } + pimpl_->pending_result_responses[sequence_number](response); + pimpl_->pending_result_responses.erase(sequence_number); +} + +void +ClientBase::send_result_request(std::shared_ptr request, ResponseCallback callback) +{ + std::lock_guard guard(pimpl_->result_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_result_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request"); + } + assert(pimpl_->pending_result_responses.count(sequence_number) == 0); + pimpl_->pending_result_responses[sequence_number] = callback; +} + +void +ClientBase::handle_cancel_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::lock_guard guard(pimpl_->cancel_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring..."); + return; + } + pimpl_->pending_cancel_responses[sequence_number](response); + pimpl_->pending_cancel_responses.erase(sequence_number); +} + +void +ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback callback) +{ + std::lock_guard guard(pimpl_->cancel_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_cancel_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request"); + } + assert(pimpl_->pending_cancel_responses.count(sequence_number) == 0); + pimpl_->pending_cancel_responses[sequence_number] = callback; +} + +GoalUUID +ClientBase::generate_goal_id() +{ + GoalUUID goal_id; + // TODO(hidmic): Do something better than this for UUID generation. + // std::generate( + // goal_id.uuid.begin(), goal_id.uuid.end(), + // std::ref(pimpl_->random_bytes_generator)); + std::generate( + goal_id.begin(), goal_id.end(), + std::ref(pimpl_->random_bytes_generator)); + return goal_id; +} + +void +ClientBase::execute() +{ + if (pimpl_->is_feedback_ready) { + std::shared_ptr feedback_message = this->create_feedback_message(); + rcl_ret_t ret = rcl_action_take_feedback( + pimpl_->client_handle.get(), feedback_message.get()); + pimpl_->is_feedback_ready = false; + if (RCL_RET_OK == ret) { + this->handle_feedback_message(feedback_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); + } + } else if (pimpl_->is_status_ready) { + std::shared_ptr status_message = this->create_status_message(); + rcl_ret_t ret = rcl_action_take_status( + pimpl_->client_handle.get(), status_message.get()); + pimpl_->is_status_ready = false; + if (RCL_RET_OK == ret) { + this->handle_status_message(status_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); + } + } else if (pimpl_->is_goal_response_ready) { + rmw_request_id_t response_header; + std::shared_ptr goal_response = this->create_goal_response(); + rcl_ret_t ret = rcl_action_take_goal_response( + pimpl_->client_handle.get(), &response_header, goal_response.get()); + pimpl_->is_goal_response_ready = false; + if (RCL_RET_OK == ret) { + this->handle_goal_response(response_header, goal_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response"); + } + } else if (pimpl_->is_result_response_ready) { + rmw_request_id_t response_header; + std::shared_ptr result_response = this->create_result_response(); + rcl_ret_t ret = rcl_action_take_result_response( + pimpl_->client_handle.get(), &response_header, result_response.get()); + pimpl_->is_result_response_ready = false; + if (RCL_RET_OK == ret) { + this->handle_result_response(response_header, result_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response"); + } + } else if (pimpl_->is_cancel_response_ready) { + rmw_request_id_t response_header; + std::shared_ptr cancel_response = this->create_cancel_response(); + rcl_ret_t ret = rcl_action_take_cancel_response( + pimpl_->client_handle.get(), &response_header, cancel_response.get()); + pimpl_->is_cancel_response_ready = false; + if (RCL_RET_OK == ret) { + this->handle_cancel_response(response_header, cancel_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response"); + } + } else { + throw std::runtime_error("Executing action client but nothing is ready"); + } +} + +} // namespace rclcpp_action diff --git a/rclcpp-eloquent/rclcpp_action/src/qos.cpp b/rclcpp-eloquent/rclcpp_action/src/qos.cpp new file mode 100644 index 0000000..615b825 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/src/qos.cpp @@ -0,0 +1,28 @@ +// Copyright 2019 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 + +namespace rclcpp_action +{ + +DefaultActionStatusQoS::DefaultActionStatusQoS() +: rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rcl_action_qos_profile_status_default)) +{ + this->get_rmw_qos_profile() = rcl_action_qos_profile_status_default; +} + +} // namespace rclcpp_action diff --git a/rclcpp-eloquent/rclcpp_action/src/server.cpp b/rclcpp-eloquent/rclcpp_action/src/server.cpp new file mode 100644 index 0000000..df21d34 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/src/server.cpp @@ -0,0 +1,560 @@ +// Copyright 2018 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 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using rclcpp_action::ServerBase; +using rclcpp_action::GoalUUID; + +namespace rclcpp_action +{ +class ServerBaseImpl +{ +public: + ServerBaseImpl( + rclcpp::Clock::SharedPtr clock, + rclcpp::Logger logger + ) + : clock_(clock), logger_(logger) + { + } + + // Lock everything except user callbacks + std::recursive_mutex reentrant_mutex_; + + std::shared_ptr action_server_; + + rclcpp::Clock::SharedPtr clock_; + + size_t num_subscriptions_ = 0; + size_t num_timers_ = 0; + size_t num_clients_ = 0; + size_t num_services_ = 0; + size_t num_guard_conditions_ = 0; + + bool goal_request_ready_ = false; + bool cancel_request_ready_ = false; + bool result_request_ready_ = false; + bool goal_expired_ = false; + + // Results to be kept until the goal expires after reaching a terminal state + std::unordered_map> goal_results_; + // Requests for results are kept until a result becomes available + std::unordered_map> result_requests_; + // rcl goal handles are kept so api to send result doesn't try to access freed memory + std::unordered_map> goal_handles_; + + rclcpp::Logger logger_; +}; +} // namespace rclcpp_action + +ServerBase::ServerBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & name, + const rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options +) +: pimpl_(new ServerBaseImpl( + node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) +{ + auto deleter = [node_base](rcl_action_server_t * ptr) + { + if (nullptr != ptr) { + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node); + (void)ret; + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_server_t in deleter"); + } + delete ptr; + }; + + pimpl_->action_server_.reset(new rcl_action_server_t, deleter); + *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server(); + + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); + + rcl_ret_t ret = rcl_action_server_init( + pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + ret = rcl_action_server_wait_set_get_num_entities( + pimpl_->action_server_.get(), + &pimpl_->num_subscriptions_, + &pimpl_->num_guard_conditions_, + &pimpl_->num_timers_, + &pimpl_->num_clients_, + &pimpl_->num_services_); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +ServerBase::~ServerBase() +{ +} + +size_t +ServerBase::get_number_of_ready_subscriptions() +{ + return pimpl_->num_subscriptions_; +} + +size_t +ServerBase::get_number_of_ready_timers() +{ + return pimpl_->num_timers_; +} + +size_t +ServerBase::get_number_of_ready_clients() +{ + return pimpl_->num_clients_; +} + +size_t +ServerBase::get_number_of_ready_services() +{ + return pimpl_->num_services_; +} + +size_t +ServerBase::get_number_of_ready_guard_conditions() +{ + return pimpl_->num_guard_conditions_; +} + +bool +ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(pimpl_->reentrant_mutex_); + rcl_ret_t ret = rcl_action_wait_set_add_action_server( + wait_set, pimpl_->action_server_.get(), NULL); + return RCL_RET_OK == ret; +} + +bool +ServerBase::is_ready(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(pimpl_->reentrant_mutex_); + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + pimpl_->action_server_.get(), + &pimpl_->goal_request_ready_, + &pimpl_->cancel_request_ready_, + &pimpl_->result_request_ready_, + &pimpl_->goal_expired_); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + return pimpl_->goal_request_ready_ || + pimpl_->cancel_request_ready_ || + pimpl_->result_request_ready_ || + pimpl_->goal_expired_; +} + +void +ServerBase::execute() +{ + if (pimpl_->goal_request_ready_) { + execute_goal_request_received(); + } else if (pimpl_->cancel_request_ready_) { + execute_cancel_request_received(); + } else if (pimpl_->result_request_ready_) { + execute_result_request_received(); + } else if (pimpl_->goal_expired_) { + execute_check_expired_goals(); + } else { + throw std::runtime_error("Executing action server but nothing is ready"); + } +} + +void +ServerBase::execute_goal_request_received() +{ + rcl_ret_t ret; + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rmw_request_id_t request_header; + + std::lock_guard lock(pimpl_->reentrant_mutex_); + + std::shared_ptr message = create_goal_request(); + ret = rcl_action_take_goal_request( + pimpl_->action_server_.get(), + &request_header, + message.get()); + + pimpl_->goal_request_ready_ = false; + + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + GoalUUID uuid = get_goal_id_from_goal_request(message.get()); + convert(uuid, &goal_info); + + // Call user's callback, getting the user's response and a ros message to send back + auto response_pair = call_handle_goal_callback(uuid, message); + + ret = rcl_action_send_goal_response( + pimpl_->action_server_.get(), + &request_header, + response_pair.second.get()); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + const auto status = response_pair.first; + + // if goal is accepted, create a goal handle, and store it + if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) { + RCLCPP_DEBUG(pimpl_->logger_, "Accepted goal %s", to_string(uuid).c_str()); + // rcl_action will set time stamp + auto deleter = [](rcl_action_goal_handle_t * ptr) + { + if (nullptr != ptr) { + rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); + (void)fail_ret; + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_goal_handle_t in deleter"); + delete ptr; + } + }; + rcl_action_goal_handle_t * rcl_handle; + rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); + if (!rcl_handle) { + throw std::runtime_error("Failed to accept new goal\n"); + } + + std::shared_ptr handle(new rcl_action_goal_handle_t, deleter); + // Copy out goal handle since action server storage disappears when it is fini'd + *handle = *rcl_handle; + + pimpl_->goal_handles_[uuid] = handle; + + if (GoalResponse::ACCEPT_AND_EXECUTE == status) { + // Change status to executing + ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + // publish status since a goal's state has changed (was accepted or has begun execution) + publish_status(); + + // Tell user to start executing action + call_goal_accepted_callback(handle, uuid, message); + } +} + +void +ServerBase::execute_cancel_request_received() +{ + rcl_ret_t ret; + rmw_request_id_t request_header; + + // Initialize cancel request + auto request = std::make_shared(); + + std::lock_guard lock(pimpl_->reentrant_mutex_); + ret = rcl_action_take_cancel_request( + pimpl_->action_server_.get(), + &request_header, + request.get()); + + pimpl_->cancel_request_ready_ = false; + + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // Convert c++ message to C message + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info); + cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; + cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; + + // Get a list of goal info that should be attempted to be cancelled + rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); + + ret = rcl_action_process_cancel_request( + pimpl_->action_server_.get(), + &cancel_request, + &cancel_response); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + RCLCPP_SCOPE_EXIT( + { + ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to fini cancel response"); + } + }); + + auto response = std::make_shared(); + + response->return_code = cancel_response.msg.return_code; + auto & goals = cancel_response.msg.goals_canceling; + // For each canceled goal, call cancel callback + for (size_t i = 0; i < goals.size; ++i) { + const rcl_action_goal_info_t & goal_info = goals.data[i]; + GoalUUID uuid; + convert(goal_info, &uuid); + auto response_code = call_handle_cancel_callback(uuid); + if (CancelResponse::ACCEPT == response_code) { + action_msgs::msg::GoalInfo cpp_info; + cpp_info.goal_id.uuid = uuid; + cpp_info.stamp.sec = goal_info.stamp.sec; + cpp_info.stamp.nanosec = goal_info.stamp.nanosec; + response->goals_canceling.push_back(cpp_info); + } + } + + // If the user rejects all individual requests to cancel goals, + // then we consider the top-level cancel request as rejected. + if (goals.size >= 1u && 0u == response->goals_canceling.size()) { + response->return_code = action_msgs::srv::CancelGoal::Response::ERROR_REJECTED; + } + + if (!response->goals_canceling.empty()) { + // at least one goal state changed, publish a new status message + publish_status(); + } + + ret = rcl_action_send_cancel_response( + pimpl_->action_server_.get(), &request_header, response.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerBase::execute_result_request_received() +{ + rcl_ret_t ret; + // Get the result request message + rmw_request_id_t request_header; + std::shared_ptr result_request = create_result_request(); + std::lock_guard lock(pimpl_->reentrant_mutex_); + ret = rcl_action_take_result_request( + pimpl_->action_server_.get(), &request_header, result_request.get()); + + pimpl_->result_request_ready_ = false; + + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + std::shared_ptr result_response; + + // check if the goal exists + GoalUUID uuid = get_goal_id_from_result_request(result_request.get()); + rcl_action_goal_info_t goal_info; + convert(uuid, &goal_info); + bool goal_exists; + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); + if (!goal_exists) { + // Goal does not exists + result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN); + } else { + // Goal exists, check if a result is already available + auto iter = pimpl_->goal_results_.find(uuid); + if (iter != pimpl_->goal_results_.end()) { + result_response = iter->second; + } + } + + if (result_response) { + // Send the result now + ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_response.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } else { + // Store the request so it can be responded to later + pimpl_->result_requests_[uuid].push_back(request_header); + } +} + +void +ServerBase::execute_check_expired_goals() +{ + // Allocate expecting only one goal to expire at a time + rcl_action_goal_info_t expired_goals[1]; + size_t num_expired = 1; + + // Loop in case more than 1 goal expired + while (num_expired > 0u) { + std::lock_guard lock(pimpl_->reentrant_mutex_); + rcl_ret_t ret; + ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } else if (num_expired) { + // A goal expired! + GoalUUID uuid; + convert(expired_goals[0], &uuid); + RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); + pimpl_->goal_results_.erase(uuid); + pimpl_->result_requests_.erase(uuid); + pimpl_->goal_handles_.erase(uuid); + } + } +} + +void +ServerBase::publish_status() +{ + rcl_ret_t ret; + + // Get all goal handles known to C action server + rcl_action_goal_handle_t ** goal_handles = NULL; + size_t num_goals = 0; + ret = rcl_action_server_get_goal_handles( + pimpl_->action_server_.get(), &goal_handles, &num_goals); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + auto status_msg = std::make_shared(); + status_msg->status_list.reserve(num_goals); + // Populate a c++ status message with the goals and their statuses + rcl_action_goal_status_array_t c_status_array = + rcl_action_get_zero_initialized_goal_status_array(); + ret = rcl_action_get_goal_status_array(pimpl_->action_server_.get(), &c_status_array); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + RCLCPP_SCOPE_EXIT( + { + ret = rcl_action_goal_status_array_fini(&c_status_array); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message"); + } + }); + + for (size_t i = 0; i < c_status_array.msg.status_list.size; ++i) { + auto & c_status_msg = c_status_array.msg.status_list.data[i]; + + action_msgs::msg::GoalStatus msg; + msg.status = c_status_msg.status; + // Convert C goal info to C++ goal info + convert(c_status_msg.goal_info, &msg.goal_info.goal_id.uuid); + msg.goal_info.stamp.sec = c_status_msg.goal_info.stamp.sec; + msg.goal_info.stamp.nanosec = c_status_msg.goal_info.stamp.nanosec; + + status_msg->status_list.push_back(msg); + } + + // Publish the message through the status publisher + ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_msg) +{ + // Check that the goal exists + rcl_action_goal_info_t goal_info; + convert(uuid, &goal_info); + std::lock_guard lock(pimpl_->reentrant_mutex_); + bool goal_exists; + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); + + if (!goal_exists) { + throw std::runtime_error("Asked to publish result for goal that does not exist"); + } + + pimpl_->goal_results_[uuid] = result_msg; + + // if there are clients who already asked for the result, send it to them + auto iter = pimpl_->result_requests_.find(uuid); + if (iter != pimpl_->result_requests_.end()) { + for (auto & request_header : iter->second) { + rcl_ret_t ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_msg.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + } +} + +void +ServerBase::notify_goal_terminal_state() +{ + std::lock_guard lock(pimpl_->reentrant_mutex_); + rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerBase::publish_feedback(std::shared_ptr feedback_msg) +{ + std::lock_guard lock(pimpl_->reentrant_mutex_); + rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); + } +} diff --git a/rclcpp-eloquent/rclcpp_action/src/server_goal_handle.cpp b/rclcpp-eloquent/rclcpp_action/src/server_goal_handle.cpp new file mode 100644 index 0000000..f0709e1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/src/server_goal_handle.cpp @@ -0,0 +1,140 @@ +// Copyright 2018 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 + +#include +#include + +#include + +namespace rclcpp_action +{ +ServerGoalHandleBase::~ServerGoalHandleBase() +{ +} + +bool +ServerGoalHandleBase::is_canceling() const +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_CANCELING == state; +} + +bool +ServerGoalHandleBase::is_active() const +{ + std::lock_guard lock(rcl_handle_mutex_); + return rcl_action_goal_handle_is_active(rcl_handle_.get()); +} + +bool +ServerGoalHandleBase::is_executing() const +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_EXECUTING == state; +} + +void +ServerGoalHandleBase::_abort() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_succeed() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_cancel_goal() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_canceled() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_execute() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +bool +ServerGoalHandleBase::try_canceling() noexcept +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret; + // Check if the goal is cancelable + const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get()); + if (is_cancelable) { + // Transition to CANCELING + ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL); + if (RCL_RET_OK != ret) { + return false; + } + } + + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + // Get the current state + ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + return false; + } + + // If it's canceling, cancel it + if (GOAL_STATE_CANCELING == state) { + ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED); + return RCL_RET_OK == ret; + } + + return false; +} +} // namespace rclcpp_action diff --git a/rclcpp-eloquent/rclcpp_action/src/types.cpp b/rclcpp-eloquent/rclcpp_action/src/types.cpp new file mode 100644 index 0000000..7737027 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/src/types.cpp @@ -0,0 +1,48 @@ +// Copyright 2018 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 "rclcpp_action/types.hpp" + +#include +#include + +namespace rclcpp_action +{ +std::string +to_string(const GoalUUID & goal_id) +{ + std::stringstream stream; + stream << std::hex; + for (const auto & element : goal_id) { + stream << static_cast(element); + } + return stream.str(); +} + +void +convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) +{ + for (size_t i = 0; i < 16; ++i) { + info->goal_id.uuid[i] = goal_id[i]; + } +} + +void +convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id) +{ + for (size_t i = 0; i < 16; ++i) { + (*goal_id)[i] = info.goal_id.uuid[i]; + } +} +} // namespace rclcpp_action diff --git a/rclcpp-eloquent/rclcpp_action/test/test_client.cpp b/rclcpp-eloquent/rclcpp_action/test/test_client.cpp new file mode 100644 index 0000000..82a6dbd --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/test/test_client.cpp @@ -0,0 +1,666 @@ +// Copyright 2018 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 +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp_action/exceptions.hpp" +#include "rclcpp_action/create_client.hpp" +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/qos.hpp" +#include "rclcpp_action/types.hpp" + +using namespace std::chrono_literals; + +const auto WAIT_FOR_SERVER_TIMEOUT = 10s; + +class TestClient : public ::testing::Test +{ +protected: + using ActionType = test_msgs::action::Fibonacci; + using ActionGoal = ActionType::Goal; + using ActionGoalHandle = rclcpp_action::ClientGoalHandle; + using ActionGoalRequestService = ActionType::Impl::SendGoalService; + using ActionGoalRequest = ActionGoalRequestService::Request; + using ActionGoalResponse = ActionGoalRequestService::Response; + using ActionGoalResultService = ActionType::Impl::GetResultService; + using ActionGoalResultRequest = ActionGoalResultService::Request; + using ActionGoalResultResponse = ActionGoalResultService::Response; + using ActionCancelGoalService = ActionType::Impl::CancelGoalService; + using ActionCancelGoalRequest = ActionType::Impl::CancelGoalService::Request; + using ActionCancelGoalResponse = ActionType::Impl::CancelGoalService::Response; + using ActionStatusMessage = ActionType::Impl::GoalStatusMessage; + using ActionFeedbackMessage = ActionType::Impl::FeedbackMessage; + using ActionFeedback = ActionType::Feedback; + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + rcl_allocator_t allocator = rcl_get_default_allocator(); + + server_node = std::make_shared(server_node_name, namespace_name); + + char * goal_service_name = nullptr; + rcl_ret_t ret = rcl_action_get_goal_service_name( + action_name, allocator, &goal_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + goal_service = server_node->create_service( + goal_service_name, + [this]( + const ActionGoalRequest::SharedPtr request, + ActionGoalResponse::SharedPtr response) + { + response->stamp = clock.now(); + response->accepted = (request->goal.order >= 0); + if (response->accepted) { + goals[request->goal_id.uuid] = {request, response}; + } + }); + ASSERT_TRUE(goal_service != nullptr); + allocator.deallocate(goal_service_name, allocator.state); + + char * result_service_name = nullptr; + ret = rcl_action_get_result_service_name( + action_name, allocator, &result_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + result_service = server_node->create_service( + result_service_name, + [this]( + const ActionGoalResultRequest::SharedPtr request, + ActionGoalResultResponse::SharedPtr response) + { + if (goals.count(request->goal_id.uuid) == 1) { + auto goal_request = goals[request->goal_id.uuid].first; + auto goal_response = goals[request->goal_id.uuid].second; + ActionStatusMessage status_message; + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; + status_message.status_list.push_back(goal_status); + status_publisher->publish(status_message); + client_executor.spin_once(); + ActionFeedbackMessage feedback_message; + feedback_message.goal_id.uuid = goal_request->goal_id.uuid; + feedback_message.feedback.sequence.push_back(0); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + if (goal_request->goal.order > 0) { + feedback_message.feedback.sequence.push_back(1); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + for (int i = 1; i < goal_request->goal.order; ++i) { + feedback_message.feedback.sequence.push_back( + feedback_message.feedback.sequence[i] + + feedback_message.feedback.sequence[i - 1]); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + } + } + goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + status_message.status_list[0] = goal_status; + status_publisher->publish(status_message); + client_executor.spin_once(); + response->result.sequence = feedback_message.feedback.sequence; + response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + goals.erase(request->goal_id.uuid); + } else { + response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN; + } + }); + ASSERT_TRUE(result_service != nullptr); + allocator.deallocate(result_service_name, allocator.state); + + char * cancel_service_name = nullptr; + ret = rcl_action_get_cancel_service_name( + action_name, allocator, &cancel_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + cancel_service = server_node->create_service( + cancel_service_name, + [this]( + const ActionCancelGoalRequest::SharedPtr request, + ActionCancelGoalResponse::SharedPtr response) + { + rclcpp_action::GoalUUID zero_uuid; + std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); + const rclcpp::Time cancel_stamp = request->goal_info.stamp; + bool cancel_all = ( + request->goal_info.goal_id.uuid == zero_uuid && + cancel_stamp == zero_stamp); + ActionStatusMessage status_message; + auto it = goals.begin(); + while (it != goals.end()) { + auto goal_request = it->second.first; + auto goal_response = it->second.second; + const rclcpp::Time goal_stamp = goal_response->stamp; + bool cancel_this = ( + request->goal_info.goal_id.uuid == goal_request->goal_id.uuid || + cancel_stamp > goal_stamp); + if (cancel_all || cancel_this) { + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED; + status_message.status_list.push_back(goal_status); + response->goals_canceling.push_back(goal_status.goal_info); + it = goals.erase(it); + } else { + ++it; + } + } + status_publisher->publish(status_message); + client_executor.spin_once(); + }); + ASSERT_TRUE(cancel_service != nullptr); + allocator.deallocate(cancel_service_name, allocator.state); + + char * feedback_topic_name = nullptr; + ret = rcl_action_get_feedback_topic_name( + action_name, allocator, &feedback_topic_name); + ASSERT_EQ(RCL_RET_OK, ret); + feedback_publisher = + server_node->create_publisher(feedback_topic_name, 10); + ASSERT_TRUE(feedback_publisher != nullptr); + allocator.deallocate(feedback_topic_name, allocator.state); + + char * status_topic_name = nullptr; + ret = rcl_action_get_status_topic_name( + action_name, allocator, &status_topic_name); + ASSERT_EQ(RCL_RET_OK, ret); + status_publisher = server_node->create_publisher( + status_topic_name, rclcpp_action::DefaultActionStatusQoS()); + ASSERT_TRUE(status_publisher != nullptr); + allocator.deallocate(status_topic_name, allocator.state); + server_executor.add_node(server_node); + + client_node = std::make_shared(client_node_name, namespace_name); + client_executor.add_node(client_node); + + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle())); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1))); + } + + void TearDown() + { + status_publisher.reset(); + feedback_publisher.reset(); + cancel_service.reset(); + result_service.reset(); + goal_service.reset(); + server_node.reset(); + client_node.reset(); + } + + template + void dual_spin_until_future_complete(std::shared_future & future) + { + std::future_status status; + do { + server_executor.spin_some(); + client_executor.spin_some(); + status = future.wait_for(std::chrono::seconds(0)); + } while (std::future_status::ready != status); + } + + rclcpp::Clock clock{RCL_ROS_TIME}; + const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME}; + + rclcpp::Node::SharedPtr server_node; + rclcpp::executors::SingleThreadedExecutor server_executor; + rclcpp::Node::SharedPtr client_node; + rclcpp::executors::SingleThreadedExecutor client_executor; + const char * const server_node_name{"fibonacci_action_test_server"}; + const char * const client_node_name{"fibonacci_action_test_client"}; + const char * const namespace_name{"/rclcpp_action/test/client"}; + const char * const action_name{"fibonacci_test"}; + + std::map< + rclcpp_action::GoalUUID, + std::pair< + typename ActionGoalRequest::SharedPtr, + typename ActionGoalResponse::SharedPtr>> goals; + typename rclcpp::Service::SharedPtr goal_service; + typename rclcpp::Service::SharedPtr result_service; + typename rclcpp::Service::SharedPtr cancel_service; + typename rclcpp::Publisher::SharedPtr feedback_publisher; + typename rclcpp::Publisher::SharedPtr status_publisher; +}; + +TEST_F(TestClient, construction_and_destruction) +{ + ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); +} + +TEST_F(TestClient, async_send_goal_no_callbacks) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal bad_goal; + bad_goal.order = -5; + auto future_goal_handle = action_client->async_send_goal(bad_goal); + dual_spin_until_future_complete(future_goal_handle); + EXPECT_EQ(nullptr, future_goal_handle.get().get()); + + ActionGoal good_goal; + good_goal.order = 5; + future_goal_handle = action_client->async_send_goal(good_goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError); +} + +TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + ASSERT_EQ(6ul, wrapped_result.result->sequence.size()); + EXPECT_EQ(0, wrapped_result.result->sequence[0]); + EXPECT_EQ(1, wrapped_result.result->sequence[1]); + EXPECT_EQ(5, wrapped_result.result->sequence[5]); +} + +TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + bool goal_response_received = false; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.goal_response_callback = + [&goal_response_received] + (std::shared_future future) mutable + { + auto goal_handle = future.get(); + if (goal_handle) { + goal_response_received = true; + } + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_response_received); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + +TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + int feedback_count = 0; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.feedback_callback = + [&feedback_count]( + typename ActionGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) mutable + { + (void)goal_handle; + (void)feedback; + feedback_count++; + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_TRUE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); + EXPECT_EQ(5, feedback_count); +} + +TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + bool result_callback_received = false; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.result_callback = + [&result_callback_received]( + const typename ActionGoalHandle::WrappedResult & result) mutable + { + if ( + rclcpp_action::ResultCode::SUCCEEDED == result.code && + result.result->sequence.size() == 5u) + { + result_callback_received = true; + } + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_TRUE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + +TEST_F(TestClient, async_get_result_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + bool result_callback_received = false; + auto future_result = action_client->async_get_result( + goal_handle, + [&result_callback_received]( + const typename ActionGoalHandle::WrappedResult & result) mutable + { + if ( + rclcpp_action::ResultCode::SUCCEEDED == result.code && + result.result->sequence.size() == 5u) + { + result_callback_received = true; + } + }); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + +TEST_F(TestClient, async_cancel_one_goal) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + + auto future_cancel = action_client->async_cancel_goal(goal_handle); + dual_spin_until_future_complete(future_cancel); + ActionCancelGoalResponse::SharedPtr cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); +} + +TEST_F(TestClient, async_cancel_one_goal_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + + bool cancel_response_received = false; + auto future_cancel = action_client->async_cancel_goal( + goal_handle, + [&cancel_response_received, goal_handle]( + ActionCancelGoalResponse::SharedPtr response) mutable + { + if ( + ActionCancelGoalResponse::ERROR_NONE == response->return_code && + 1ul == response->goals_canceling.size() && + goal_handle->get_goal_id() == response->goals_canceling[0].goal_id.uuid) + { + cancel_response_received = true; + } + }); + dual_spin_until_future_complete(future_cancel); + auto cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_TRUE(cancel_response_received); +} + +TEST_F(TestClient, async_cancel_all_goals) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { + goal_handle0.swap(goal_handle1); + } + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + auto future_cancel_all = action_client->async_cancel_all_goals(); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); +} + +TEST_F(TestClient, async_cancel_all_goals_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { + goal_handle0.swap(goal_handle1); + } + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + bool cancel_callback_received = false; + auto future_cancel_all = action_client->async_cancel_all_goals( + [&cancel_callback_received, goal_handle0, goal_handle1]( + ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 2ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid && + goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + EXPECT_TRUE(cancel_callback_received); + ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); +} + +TEST_F(TestClient, async_cancel_some_goals) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + auto future_cancel_some = + action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); + dual_spin_until_future_complete(future_cancel_some); + auto cancel_response = future_cancel_some.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); +} + +TEST_F(TestClient, async_cancel_some_goals_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + bool cancel_callback_received = false; + auto future_cancel_some = action_client->async_cancel_goals_before( + goal_handle1->get_goal_stamp(), + [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 1ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_some); + auto cancel_response = future_cancel_some.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + EXPECT_TRUE(cancel_callback_received); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); +} diff --git a/rclcpp-eloquent/rclcpp_action/test/test_server.cpp b/rclcpp-eloquent/rclcpp_action/test/test_server.cpp new file mode 100644 index 0000000..35e2c4e --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/test/test_server.cpp @@ -0,0 +1,792 @@ +// Copyright 2018 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 +#include +#include + +#include + +#include +#include + +#include "rclcpp_action/create_server.hpp" +#include "rclcpp_action/server.hpp" + +using Fibonacci = test_msgs::action::Fibonacci; +using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response; +using GoalUUID = rclcpp_action::GoalUUID; + +class TestServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + std::shared_ptr + send_goal_request(rclcpp::Node::SharedPtr node, GoalUUID uuid) + { + auto client = node->create_client( + "fibonacci/_action/send_goal"); + if (!client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("send goal service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = client->async_send_request(request); + if ( + rclcpp::executor::FutureReturnCode::SUCCESS != + rclcpp::spin_until_future_complete(node, future)) + { + throw std::runtime_error("send goal future didn't complete succesfully"); + } + return request; + } + + CancelResponse::SharedPtr + send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid) + { + auto cancel_client = node->create_client( + "fibonacci/_action/cancel_goal"); + if (!cancel_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("cancel goal service didn't become available"); + } + auto request = std::make_shared(); + request->goal_info.goal_id.uuid = uuid; + auto future = cancel_client->async_send_request(request); + if ( + rclcpp::executor::FutureReturnCode::SUCCESS != + rclcpp::spin_until_future_complete(node, future)) + { + throw std::runtime_error("cancel goal future didn't complete succesfully"); + } + return future.get(); + } +}; + +TEST_F(TestServer, construction_and_destruction) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; +} + +TEST_F(TestServer, handle_goal_called) +{ + auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); + GoalUUID received_uuid; + + auto handle_goal = [&received_uuid]( + const GoalUUID & uuid, std::shared_ptr) + { + received_uuid = uuid; + return rclcpp_action::GoalResponse::REJECT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + + // Create a client that calls the goal request service + // Make sure the UUID received is the same as the one sent + + auto client = node->create_client( + "fibonacci/_action/send_goal"); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + + auto request = std::make_shared(); + + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + request->goal_id.uuid = uuid; + + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + ASSERT_EQ(uuid, received_uuid); +} + +TEST_F(TestServer, handle_accepted_called) +{ + auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_accepted"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + handle_accepted); + (void)as; + + auto request = send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + ASSERT_TRUE(received_handle->is_active()); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_EQ(request->goal, *(received_handle->get_goal())); +} + +TEST_F(TestServer, handle_cancel_called) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + send_cancel_request(node, uuid); + EXPECT_TRUE(received_handle->is_canceling()); +} + +TEST_F(TestServer, handle_cancel_reject) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + auto response_ptr = send_cancel_request(node, uuid); + EXPECT_FALSE(received_handle->is_canceling()); + EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code); +} + +TEST_F(TestServer, handle_cancel_unknown_goal) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + const GoalUUID unknown_uuid{{11, 22, 33, 44, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + auto response_ptr = send_cancel_request(node, unknown_uuid); + EXPECT_FALSE(received_handle->is_canceling()); + EXPECT_EQ(CancelResponse::ERROR_UNKNOWN_GOAL_ID, response_ptr->return_code); +} + +TEST_F(TestServer, handle_cancel_terminated_goal) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + handle->succeed(std::make_shared()); + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + auto response_ptr = send_cancel_request(node, uuid); + EXPECT_FALSE(received_handle->is_canceling()); + EXPECT_EQ(CancelResponse::ERROR_GOAL_TERMINATED, response_ptr->return_code); +} + +TEST_F(TestServer, publish_status_accepted) +{ + auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() != 1u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + // Not sure whether accepted will come through because not sure when subscriber will match + for (auto & msg : received_msgs) { + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); + auto status = msg->status_list.at(0).status; + if (action_msgs::msg::GoalStatus::STATUS_ACCEPTED == status) { + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ACCEPTED, status); + } else { + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_EXECUTING, status); + } + } +} + +TEST_F(TestServer, publish_status_canceling) +{ + auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + send_cancel_request(node, uuid); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELING, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_canceled) +{ + auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + send_cancel_request(node, uuid); + + received_handle->canceled(std::make_shared()); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 3u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_succeeded) +{ + auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + received_handle->succeed(std::make_shared()); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_aborted) +{ + auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + received_handle->abort(std::make_shared()); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ABORTED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_feedback) +{ + auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); + const GoalUUID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to feedback messages + using FeedbackT = Fibonacci::Impl::FeedbackMessage; + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::SharedPtr msg) + { + received_msgs.push_back(msg); + }); + + send_goal_request(node, uuid); + + auto sent_message = std::make_shared(); + sent_message->sequence = {1, 1, 2, 3, 5}; + received_handle->publish_feedback(sent_message); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 1u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_EQ(1u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(sent_message->sequence, msg->feedback.sequence); +} + +TEST_F(TestServer, get_result) +{ + auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + // Send result request + auto result_client = node->create_client( + "fibonacci/_action/get_result"); + if (!result_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("get result service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = result_client->async_send_request(request); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + received_handle->succeed(result); + + // Wait for the result request to be received + ASSERT_EQ( + rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); +} + +TEST_F(TestServer, deferred_execution) +{ + auto node = std::make_shared("defer_exec", "/rclcpp_action/defer_exec"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + EXPECT_TRUE(received_handle->is_active()); + EXPECT_FALSE(received_handle->is_executing()); + received_handle->execute(); + EXPECT_TRUE(received_handle->is_executing()); +} diff --git a/rclcpp-eloquent/rclcpp_action/test/test_traits.cpp b/rclcpp-eloquent/rclcpp_action/test/test_traits.cpp new file mode 100644 index 0000000..38016c1 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_action/test/test_traits.cpp @@ -0,0 +1,99 @@ +// Copyright 2019 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 + +using rosidl_generator_traits::is_message; +using rosidl_generator_traits::is_service; +using rosidl_generator_traits::is_action; +using rosidl_generator_traits::is_action_goal; +using rosidl_generator_traits::is_action_result; +using rosidl_generator_traits::is_action_feedback; + +TEST(TestActionTraits, is_action) { + using Fibonacci = test_msgs::action::Fibonacci; + + // Top level definition is an action + EXPECT_FALSE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_TRUE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + // Goal is an action_goal as well as a message + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_TRUE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + // Result is an action_result as well as a message + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_TRUE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + // Feedback is an action_feedback as well as a message + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_TRUE(is_action_feedback()); +} + +TEST(TestActionTraits, is_action_impl) { + using Fibonacci = test_msgs::action::Fibonacci; + + // Test traits on some of the internal implementation of actions + EXPECT_FALSE(is_message()); + EXPECT_TRUE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_FALSE(is_message()); + EXPECT_TRUE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_FALSE(is_message()); + EXPECT_TRUE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); +} diff --git a/rclcpp-eloquent/rclcpp_components/CHANGELOG.rst b/rclcpp-eloquent/rclcpp_components/CHANGELOG.rst new file mode 100644 index 0000000..4d84636 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/CHANGELOG.rst @@ -0,0 +1,79 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp_components +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-12-04) +------------------ +* Added rclcpp_components Doxyfile (`#1091 `_) (`#1200 `_) +* Contributors: Alejandro Hernández Cordero + +0.8.4 (2020-01-17) +------------------ +* Remove absolute path from installed CMake code (`#948 `_) (`#950 `_) +* Contributors: Jacob Perron + +0.8.3 (2019-11-19) +------------------ + +0.8.2 (2019-11-18) +------------------ + +0.8.1 (2019-10-23) +------------------ +* Enable intra-process comm via LoadNode request. (`#871 `_) +* Aggregate all component manager API tests. (`#876 `_) +* Contributors: Michel Hidalgo + +0.8.0 (2019-09-26) +------------------ +* Force explicit --ros-args in NodeOptions::arguments(). (`#845 `_) +* Use of -r/--remap flags where appropriate. (`#834 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* fix linter issue (`#795 `_) +* Remove non-package from ament_target_dependencies() (`#793 `_) +* fix for multiple nodes not being recognized (`#790 `_) +* Cmake infrastructure for creating components (`#784 `_) +* Contributors: Dan Rose, Michel Hidalgo, Shane Loretz, Siddharth Kucheria + +0.7.5 (2019-05-30) +------------------ + +0.7.4 (2019-05-29) +------------------ +* Rename parameter options (`#745 `_) +* don't use global arguments for components loaded into the manager (`#736 `_) +* Contributors: Dirk Thomas, William Woodall + +0.7.3 (2019-05-20) +------------------ + +0.7.2 (2019-05-08) +------------------ +* Updated to support changes to ``Node::get_node_names()``. (`#698 `_) +* Contributors: jhdcs + +0.7.1 (2019-04-26) +------------------ + +0.7.0 (2019-04-14) +------------------ +* Introduce rclcpp_components to implement composition (`#665 `_) +* Contributors: Michael Carroll + +0.6.2 (2018-12-12) +------------------ + +0.6.1 (2018-12-06) +------------------ + +0.6.0 (2018-11-19) +------------------ + +0.5.1 (2018-06-28) +------------------ + +0.5.0 (2018-06-25) +------------------ + +0.4.0 (2017-12-08) +------------------ diff --git a/rclcpp-eloquent/rclcpp_components/CMakeLists.txt b/rclcpp-eloquent/rclcpp_components/CMakeLists.txt new file mode 100644 index 0000000..e7eacd6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/CMakeLists.txt @@ -0,0 +1,126 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp_components) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(class_loader REQUIRED) +find_package(composition_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) + +include_directories(include) + +add_library( + component_manager + STATIC + src/component_manager.cpp +) +ament_target_dependencies(component_manager + "ament_index_cpp" + "class_loader" + "composition_interfaces" + "rclcpp" + "rcpputils" +) + +add_executable( + component_container + src/component_container.cpp +) +target_link_libraries(component_container component_manager) +ament_target_dependencies(component_container + "rclcpp" +) + +set(node_main_template_install_dir "share/${PROJECT_NAME}") +install(FILES + src/node_main.cpp.in + DESTINATION ${node_main_template_install_dir}) + +add_executable( + component_container_mt + src/component_container_mt.cpp +) +target_link_libraries(component_container_mt component_manager) +ament_target_dependencies(component_container_mt + "rclcpp" +) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + target_link_libraries(component_container "stdc++fs") + target_link_libraries(component_container_mt "stdc++fs") +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + set(components "") + add_library(test_component SHARED test/components/test_component.cpp) + ament_target_dependencies(test_component + "class_loader" + "rclcpp") + #rclcpp_components_register_nodes(test_component "test_rclcpp_components::TestComponent") + set(components "${components}test_rclcpp_components::TestComponentFoo;$\n") + set(components "${components}test_rclcpp_components::TestComponentBar;$\n") + set(components "${components}test_rclcpp_components::TestComponentNoNode;$\n") + + file(GENERATE + OUTPUT + "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/share/ament_index/resource_index/rclcpp_components/${PROJECT_NAME}" + CONTENT "${components}") + + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") + if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") + endif() + + ament_add_gtest(test_component_manager test/test_component_manager.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$ + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_component_manager) + target_link_libraries(test_component_manager component_manager) + target_include_directories(test_component_manager PRIVATE src) + endif() + + ament_add_gtest(test_component_manager_api test/test_component_manager_api.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$ + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_component_manager_api) + target_link_libraries(test_component_manager_api component_manager) + target_include_directories(test_component_manager_api PRIVATE src) + endif() +endif() + +# Install executables +install( + TARGETS component_container component_container_mt + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +# Install include directories +install( + DIRECTORY include/ + DESTINATION include +) + +# Install cmake +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) + +# specific order: dependents before dependencies +ament_export_include_directories(include) +ament_export_dependencies(class_loader) +ament_export_dependencies(rclcpp) +ament_package(CONFIG_EXTRAS rclcpp_components-extras.cmake.in) diff --git a/rclcpp-eloquent/rclcpp_components/Doxyfile b/rclcpp-eloquent/rclcpp_components/Doxyfile new file mode 100644 index 0000000..da33050 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/Doxyfile @@ -0,0 +1,33 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_components" +PROJECT_NUMBER = master +PROJECT_BRIEF = "Package containing tools for dynamically loadable components" + +# Use these lines to include the generated logging.hpp (update install path if needed) +# Otherwise just generate for the local (non-generated header files) +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_COMPONENTS_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/class_loader.tag=http://docs.ros2.org/latest/api/class_loader/" +TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_components.tag" diff --git a/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_package_hook.cmake b/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_package_hook.cmake new file mode 100644 index 0000000..4833d02 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_package_hook.cmake @@ -0,0 +1,18 @@ +# Copyright 2019 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. + +# register node plugins +ament_index_register_resource( + "rclcpp_components" CONTENT "${_RCLCPP_COMPONENTS__NODES}") + diff --git a/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_node.cmake b/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_node.cmake new file mode 100644 index 0000000..3e38aa5 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -0,0 +1,52 @@ +# Copyright 2019 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. +# +# usage: rclcpp_components_register_node( +# PLUGIN EXECUTABLE ) +# +# Register an rclcpp component with the ament +# resource index and create an executable. +# +# :param target: the shared library target +# :type target: string +# :param PLUGIN: the plugin name +# :type PLUGIN: string +# :type EXECUTABLE: the node's executable name +# :type EXECUTABLE: string +# +macro(rclcpp_components_register_node target) + cmake_parse_arguments(ARGS "" "PLUGIN;EXECUTABLE" "" ${ARGN}) + set(component ${ARGS_PLUGIN}) + set(node ${ARGS_EXECUTABLE}) + _rclcpp_components_register_package_hook() + set(_path "lib") + set(library_name "$") + if(WIN32) + set(_path "bin") + endif() + set(_RCLCPP_COMPONENTS__NODES + "${_RCLCPP_COMPONENTS__NODES}${component};${_path}/$\n") + configure_file(${rclcpp_components_NODE_TEMPLATE} + ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_configured_${node}.cpp.in) + file(GENERATE OUTPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp + INPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_configured_${node}.cpp.in) + add_executable(${node} ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp) + ament_target_dependencies(${node} + "rclcpp" + "class_loader" + "rclcpp_components") + install(TARGETS + ${node} + DESTINATION lib/${PROJECT_NAME}) +endmacro() diff --git a/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake b/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake new file mode 100644 index 0000000..eac3841 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake @@ -0,0 +1,62 @@ +# Copyright 2019 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. + +# +# Register an rclcpp component with the ament resource index. +# +# The passed library can contain multiple nodes each registered via macro. +# +# :param target: the shared library target +# :type target: string +# :param ARGN: the unique plugin names being exported using class_loader +# :type ARGN: list of strings +# +macro(rclcpp_components_register_nodes target) + if(NOT TARGET ${target}) + message( + FATAL_ERROR + "rclcpp_components_register_nodes() first argument " + "'${target}' is not a target") + endif() + get_target_property(_target_type ${target} TYPE) + if(NOT _target_type STREQUAL "SHARED_LIBRARY") + message( + FATAL_ERROR + "rclcpp_components_register_nodes() first argument " + "'${target}' is not a shared library target") + endif() + + if(${ARGC} GREATER 0) + _rclcpp_components_register_package_hook() + set(_unique_names) + foreach(_arg ${ARGN}) + if(_arg IN_LIST _unique_names) + message( + FATAL_ERROR + "rclcpp_components_register_nodes() the plugin names " + "must be unique (multiple '${_arg}')") + endif() + list(APPEND _unique_names "${_arg}") + + if(WIN32) + set(_path "bin") + else() + set(_path "lib") + endif() + set(_RCLCPP_COMPONENTS__NODES + "${_RCLCPP_COMPONENTS__NODES}${_arg};${_path}/$\n") + endforeach() + endif() +endmacro() + diff --git a/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory.hpp new file mode 100644 index 0000000..67e6cd7 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -0,0 +1,46 @@ +// Copyright 2019 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 RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ +#define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ + +#include "rclcpp_components/node_instance_wrapper.hpp" + +namespace rclcpp_components +{ + +/// The NodeFactory interface is used by the class loader to instantiate components. +/** + * The NodeFactory interface serves two purposes: + * * It allows for classes not derived from `rclcpp::Node` to be used as components. + * * It allows derived constructors to be called when components are loaded. + */ +class NodeFactory +{ +public: + NodeFactory() = default; + + virtual ~NodeFactory() = default; + + /// Create an instance of a component + /** + * \param[in] options Additional options used in the construction of the component. + */ + virtual + NodeInstanceWrapper + create_node_instance(const rclcpp::NodeOptions & options) = 0; +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ diff --git a/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory_template.hpp new file mode 100644 index 0000000..988b803 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -0,0 +1,53 @@ +// Copyright 2019 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 RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ +#define RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ + +#include +#include + +#include "rclcpp_components/node_factory.hpp" + +namespace rclcpp_components +{ + +/// NodeFactoryTemplate is a convenience class for instantiating components. +/** + * The NodeFactoryTemplate class can be used to provide the NodeFactory interface for + * components that implement a single-argument constructor and `get_node_base_interface`. + */ +template +class NodeFactoryTemplate : public NodeFactory +{ +public: + NodeFactoryTemplate() = default; + virtual ~NodeFactoryTemplate() = default; + + /// Create an instance of a component + /** + * \param[in] options Additional options used in the construction of the component. + */ + NodeInstanceWrapper + create_node_instance(const rclcpp::NodeOptions & options) override + { + auto node = std::make_shared(options); + + return NodeInstanceWrapper( + node, std::bind(&NodeT::get_node_base_interface, node)); + } +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ diff --git a/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp new file mode 100644 index 0000000..2705e4e --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -0,0 +1,71 @@ +// Copyright 2019 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 RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__ +#define RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__ + +#include +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +namespace rclcpp_components +{ +/// The NodeInstanceWrapper encapsulates the node instance. +class NodeInstanceWrapper +{ +public: + using NodeBaseInterfaceGetter = std::function< + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr &)>; + + NodeInstanceWrapper() + : node_instance_(nullptr) + {} + + NodeInstanceWrapper( + std::shared_ptr node_instance, + NodeBaseInterfaceGetter node_base_interface_getter) + : node_instance_(node_instance), node_base_interface_getter_(node_base_interface_getter) + {} + + /// Get a type-erased pointer to the original Node instance + /** + * This is only for debugging and special cases. + * For most cases `get_node_base_interface` will be sufficient. + * + * \return Shared pointer to the encapsulated Node instance. + */ + const std::shared_ptr + get_node_instance() const + { + return node_instance_; + } + + /// Get NodeBaseInterface pointer for the encapsulated Node Instance. + /** + * \return Shared NodeBaseInterface pointer of the encapsulated Node instance. + */ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() + { + return node_base_interface_getter_(node_instance_); + } + +private: + std::shared_ptr node_instance_; + NodeBaseInterfaceGetter node_base_interface_getter_; +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__ diff --git a/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/register_node_macro.hpp b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/register_node_macro.hpp new file mode 100644 index 0000000..340d802 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/include/rclcpp_components/register_node_macro.hpp @@ -0,0 +1,38 @@ +// Copyright 2019 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 RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ +#define RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ + +#include "class_loader/class_loader.hpp" +#include "rclcpp_components/node_factory_template.hpp" + +/// Register a component that can be dynamically loaded at runtime. +/** + * The registration macro should appear once per component per library. + * The macro should appear in a single translation unit. + * + * Valid arguments for NodeClass shall: + * * Have a constructor that takes a single argument that is a `rclcpp::NodeOptions` instance. + * * Have a method of of the signature: + * `rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface` + * + * Note: NodeClass does not need to inherit from `rclcpp::Node`, but it is the easiest way. + */ +#define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \ + CLASS_LOADER_REGISTER_CLASS( \ + rclcpp_components::NodeFactoryTemplate, \ + rclcpp_components::NodeFactory) + +#endif // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ diff --git a/rclcpp-eloquent/rclcpp_components/package.xml b/rclcpp-eloquent/rclcpp_components/package.xml new file mode 100644 index 0000000..1600799 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/package.xml @@ -0,0 +1,33 @@ + + + + rclcpp_components + 0.8.5 + Package containing tools for dynamically loadable components + Michael Carroll + Apache License 2.0 + + ament_cmake_ros + + ament_index_cpp + class_loader + composition_interfaces + rclcpp + rcpputils + + ament_index_cpp + class_loader + composition_interfaces + rclcpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + launch_testing + std_msgs + + + ament_cmake + + + diff --git a/rclcpp-eloquent/rclcpp_components/rclcpp_components-extras.cmake.in b/rclcpp-eloquent/rclcpp_components/rclcpp_components-extras.cmake.in new file mode 100644 index 0000000..45a4e5a --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/rclcpp_components-extras.cmake.in @@ -0,0 +1,32 @@ +# Copyright 2019 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. + +# copied from rclcpp_components/rclcpp_components-extras.cmake + +# register ament_package() hook for node plugins once. +macro(_rclcpp_components_register_package_hook) + if(NOT DEFINED _RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED) + set(_RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED TRUE) + + find_package(ament_cmake_core QUIET REQUIRED) + ament_register_extension("ament_package" "rclcpp_components" + "rclcpp_components_package_hook.cmake") + endif() +endmacro() + +get_filename_component(@PROJECT_NAME@_SHARE_DIR "${@PROJECT_NAME@_DIR}" DIRECTORY) +set(@PROJECT_NAME@_NODE_TEMPLATE "${@PROJECT_NAME@_SHARE_DIR}/node_main.cpp.in") + +include("${rclcpp_components_DIR}/rclcpp_components_register_nodes.cmake") +include("${rclcpp_components_DIR}/rclcpp_components_register_node.cmake") diff --git a/rclcpp-eloquent/rclcpp_components/src/component_container.cpp b/rclcpp-eloquent/rclcpp_components/src/component_container.cpp new file mode 100644 index 0000000..e1f3eaf --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/src/component_container.cpp @@ -0,0 +1,29 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" + +#include "component_manager.hpp" + +int main(int argc, char * argv[]) +{ + /// Component container with a single-threaded executor. + rclcpp::init(argc, argv); + auto exec = std::make_shared(); + auto node = std::make_shared(exec); + exec->add_node(node); + exec->spin(); +} diff --git a/rclcpp-eloquent/rclcpp_components/src/component_container_mt.cpp b/rclcpp-eloquent/rclcpp_components/src/component_container_mt.cpp new file mode 100644 index 0000000..af8e3a0 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/src/component_container_mt.cpp @@ -0,0 +1,29 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" + +#include "component_manager.hpp" + +int main(int argc, char * argv[]) +{ + /// Component container with a multi-threaded executor. + rclcpp::init(argc, argv); + auto exec = std::make_shared(); + auto node = std::make_shared(exec); + exec->add_node(node); + exec->spin(); +} diff --git a/rclcpp-eloquent/rclcpp_components/src/component_manager.cpp b/rclcpp-eloquent/rclcpp_components/src/component_manager.cpp new file mode 100644 index 0000000..05bfa9d --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/src/component_manager.cpp @@ -0,0 +1,263 @@ +// Copyright 2019 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 "component_manager.hpp" + +#include +#include +#include +#include + +#include "ament_index_cpp/get_resource.hpp" +#include "rcpputils/filesystem_helper.hpp" +#include "rcpputils/split.hpp" + +using namespace std::placeholders; + +namespace rclcpp_components +{ + +ComponentManager::ComponentManager( + std::weak_ptr executor) +: Node("ComponentManager"), + executor_(executor) +{ + loadNode_srv_ = create_service( + "~/_container/load_node", + std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3)); + unloadNode_srv_ = create_service( + "~/_container/unload_node", + std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3)); + listNodes_srv_ = create_service( + "~/_container/list_nodes", + std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); +} + +ComponentManager::~ComponentManager() +{ + if (node_wrappers_.size()) { + RCLCPP_DEBUG(get_logger(), "Removing components from executor"); + if (auto exec = executor_.lock()) { + for (auto & wrapper : node_wrappers_) { + exec->remove_node(wrapper.second.get_node_base_interface()); + } + } + } +} + +std::vector +ComponentManager::get_component_resources(const std::string & package_name) const +{ + std::string content; + std::string base_path; + if ( + !ament_index_cpp::get_resource( + "rclcpp_components", package_name, content, &base_path)) + { + throw ComponentManagerException("Could not find requested resource in ament index"); + } + + std::vector resources; + std::vector lines = rcpputils::split(content, '\n', true); + for (const auto & line : lines) { + std::vector parts = rcpputils::split(line, ';'); + if (parts.size() != 2) { + throw ComponentManagerException("Invalid resource entry"); + } + + std::string library_path = parts[1]; + if (!rcpputils::fs::path(library_path).is_absolute()) { + library_path = base_path + "/" + library_path; + } + resources.push_back({parts[0], library_path}); + } + return resources; +} + +std::shared_ptr +ComponentManager::create_component_factory(const ComponentResource & resource) +{ + std::string library_path = resource.second; + std::string class_name = resource.first; + std::string fq_class_name = "rclcpp_components::NodeFactoryTemplate<" + class_name + ">"; + + class_loader::ClassLoader * loader; + if (loaders_.find(library_path) == loaders_.end()) { + RCLCPP_INFO(get_logger(), "Load Library: %s", library_path.c_str()); + try { + loaders_[library_path] = std::make_unique(library_path); + } catch (const std::exception & ex) { + throw ComponentManagerException("Failed to load library: " + std::string(ex.what())); + } catch (...) { + throw ComponentManagerException("Failed to load library"); + } + } + loader = loaders_[library_path].get(); + + auto classes = loader->getAvailableClasses(); + for (const auto & clazz : classes) { + RCLCPP_INFO(get_logger(), "Found class: %s", clazz.c_str()); + if (clazz == class_name || clazz == fq_class_name) { + RCLCPP_INFO(get_logger(), "Instantiate class: %s", clazz.c_str()); + return loader->createInstance(clazz); + } + } + return {}; +} + +void +ComponentManager::OnLoadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + + try { + auto resources = get_component_resources(request->package_name); + + for (const auto & resource : resources) { + if (resource.first != request->plugin_name) { + continue; + } + auto factory = create_component_factory(resource); + + if (factory == nullptr) { + continue; + } + + std::vector parameters; + for (const auto & p : request->parameters) { + parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); + } + + std::vector remap_rules; + remap_rules.reserve(request->remap_rules.size() * 2 + 1); + remap_rules.push_back("--ros-args"); + for (const std::string & rule : request->remap_rules) { + remap_rules.push_back("-r"); + remap_rules.push_back(rule); + } + + if (!request->node_name.empty()) { + remap_rules.push_back("-r"); + remap_rules.push_back("__node:=" + request->node_name); + } + + if (!request->node_namespace.empty()) { + remap_rules.push_back("-r"); + remap_rules.push_back("__ns:=" + request->node_namespace); + } + + auto options = rclcpp::NodeOptions() + .use_global_arguments(false) + .parameter_overrides(parameters) + .arguments(remap_rules); + + for (const auto & a : request->extra_arguments) { + const rclcpp::Parameter extra_argument = rclcpp::Parameter::from_parameter_msg(a); + if (extra_argument.get_name() == "use_intra_process_comms") { + if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + throw ComponentManagerException( + "Extra component argument 'use_intra_process_comms' must be a boolean"); + } + options.use_intra_process_comms(extra_argument.get_value()); + } + } + + auto node_id = unique_id++; + + if (0 == node_id) { + // This puts a technical limit on the number of times you can add a component. + // But even if you could add (and remove) them at 1 kHz (very optimistic rate) + // it would still be a very long time before you could exhaust the pool of id's: + // 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years + // So around 585 million years. Even at 1 GHz, it would take 585 years. + // I think it's safe to avoid trying to handle overflow. + // If we roll over then it's most likely a bug. + throw std::overflow_error("exhausted the unique ids for components in this process"); + } + + try { + node_wrappers_[node_id] = factory->create_node_instance(options); + } catch (...) { + // In the case that the component constructor throws an exception, + // rethrow into the following catch block. + throw ComponentManagerException("Component constructor threw an exception"); + } + + auto node = node_wrappers_[node_id].get_node_base_interface(); + if (auto exec = executor_.lock()) { + exec->add_node(node, true); + } + response->full_node_name = node->get_fully_qualified_name(); + response->unique_id = node_id; + response->success = true; + return; + } + RCLCPP_ERROR( + get_logger(), "Failed to find class with the requested plugin name '%s' in " + "the loaded library", + request->plugin_name.c_str()); + response->error_message = "Failed to find class with the requested plugin name."; + response->success = false; + } catch (const ComponentManagerException & ex) { + RCLCPP_ERROR(get_logger(), ex.what()); + response->error_message = ex.what(); + response->success = false; + } +} + +void +ComponentManager::OnUnloadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + + auto wrapper = node_wrappers_.find(request->unique_id); + + if (wrapper == node_wrappers_.end()) { + response->success = false; + std::stringstream ss; + ss << "No node found with unique_id: " << request->unique_id; + response->error_message = ss.str(); + RCLCPP_WARN(get_logger(), ss.str()); + } else { + if (auto exec = executor_.lock()) { + exec->remove_node(wrapper->second.get_node_base_interface()); + } + node_wrappers_.erase(wrapper); + response->success = true; + } +} + +void +ComponentManager::OnListNodes( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + (void) request; + + for (auto & wrapper : node_wrappers_) { + response->unique_ids.push_back(wrapper.first); + response->full_node_names.push_back( + wrapper.second.get_node_base_interface()->get_fully_qualified_name()); + } +} + +} // namespace rclcpp_components diff --git a/rclcpp-eloquent/rclcpp_components/src/component_manager.hpp b/rclcpp-eloquent/rclcpp_components/src/component_manager.hpp new file mode 100644 index 0000000..62d7e1e --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/src/component_manager.hpp @@ -0,0 +1,104 @@ +// Copyright 2019 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 COMPONENT_MANAGER_HPP__ +#define COMPONENT_MANAGER_HPP__ + +#include +#include +#include +#include +#include + +#include "class_loader/class_loader.hpp" + +#include "rclcpp/executor.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "composition_interfaces/srv/load_node.hpp" +#include "composition_interfaces/srv/unload_node.hpp" +#include "composition_interfaces/srv/list_nodes.hpp" + +#include "rclcpp_components/node_factory.hpp" + +namespace rclcpp_components +{ + +class ComponentManagerException : public std::runtime_error +{ +public: + explicit ComponentManagerException(const std::string & error_desc) + : std::runtime_error(error_desc) {} +}; + +class ComponentManager : public rclcpp::Node +{ +public: + using LoadNode = composition_interfaces::srv::LoadNode; + using UnloadNode = composition_interfaces::srv::UnloadNode; + using ListNodes = composition_interfaces::srv::ListNodes; + + /// Represents a component resource. + /** + * Is a pair of class name (for class loader) and library path (absolute) + */ + using ComponentResource = std::pair; + + ComponentManager( + std::weak_ptr executor); + + ~ComponentManager(); + + /// Return a list of valid loadable components in a given package. + std::vector + get_component_resources(const std::string & package_name) const; + + std::shared_ptr + create_component_factory(const ComponentResource & resource); + +private: + void + OnLoadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + void + OnUnloadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + void + OnListNodes( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +private: + std::weak_ptr executor_; + + uint64_t unique_id {1}; + std::map> loaders_; + std::map node_wrappers_; + + rclcpp::Service::SharedPtr loadNode_srv_; + rclcpp::Service::SharedPtr unloadNode_srv_; + rclcpp::Service::SharedPtr listNodes_srv_; +}; + +} // namespace rclcpp_components + +#endif // COMPONENT_MANAGER_HPP__ diff --git a/rclcpp-eloquent/rclcpp_components/src/node_main.cpp.in b/rclcpp-eloquent/rclcpp_components/src/node_main.cpp.in new file mode 100644 index 0000000..bfebc4a --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/src/node_main.cpp.in @@ -0,0 +1,66 @@ +// Copyright 2019 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 +#include + +#include "class_loader/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/node_factory.hpp" +#include "rclcpp_components/node_factory_template.hpp" + +#define NODE_MAIN_LOGGER_NAME "@node@" + +int main(int argc, char * argv[]) +{ + auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); + rclcpp::Logger logger = rclcpp::get_logger(NODE_MAIN_LOGGER_NAME); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + options.arguments(args); + std::vector loaders; + std::vector node_wrappers; + + std::string library_name = "@library_name@"; + std::string class_name = "rclcpp_components::NodeFactoryTemplate<@component@>"; + + RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str()); + auto loader = new class_loader::ClassLoader(library_name); + auto classes = loader->getAvailableClasses(); + for (auto clazz : classes) { + std::string name = clazz.c_str(); + if (!(name.compare(class_name))) { + RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str()); + auto node_factory = loader->createInstance(clazz); + auto wrapper = node_factory->create_node_instance(options); + auto node = wrapper.get_node_base_interface(); + node_wrappers.push_back(wrapper); + exec.add_node(node); + } + } + loaders.push_back(loader); + + + exec.spin(); + + for (auto wrapper : node_wrappers) { + exec.remove_node(wrapper.get_node_base_interface()); + } + node_wrappers.clear(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/rclcpp-eloquent/rclcpp_components/test/components/test_component.cpp b/rclcpp-eloquent/rclcpp_components/test/components/test_component.cpp new file mode 100644 index 0000000..dc85187 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/test/components/test_component.cpp @@ -0,0 +1,68 @@ +// Copyright 2019 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 "rclcpp/rclcpp.hpp" + +namespace test_rclcpp_components +{ +/// Simple test component +class TestComponentFoo : public rclcpp::Node +{ +public: + explicit TestComponentFoo(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_foo", options) + { + } +}; + +/// Simple test component +class TestComponentBar : public rclcpp::Node +{ +public: + explicit TestComponentBar(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_bar", options) + { + } +}; + +/// Simple test component that doesn't inherit from rclcpp::Node +class TestComponentNoNode +{ +public: + explicit TestComponentNoNode(rclcpp::NodeOptions options) + : node_("test_component_no_node", options) + { + } + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() + { + return node_.get_node_base_interface(); + } + +private: + rclcpp::Node node_; +}; + + +} // namespace test_rclcpp_components + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentFoo) +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentBar) +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentNoNode) diff --git a/rclcpp-eloquent/rclcpp_components/test/test_component_manager.cpp b/rclcpp-eloquent/rclcpp_components/test/test_component_manager.cpp new file mode 100644 index 0000000..a86bbe5 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/test/test_component_manager.cpp @@ -0,0 +1,91 @@ +// Copyright 2019 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 + +#include "component_manager.hpp" + +#include "rcpputils/filesystem_helper.hpp" + +class TestComponentManager : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST_F(TestComponentManager, get_component_resources_invalid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + EXPECT_THROW( + manager->get_component_resources("invalid_package"), + rclcpp_components::ComponentManagerException); +} + +TEST_F(TestComponentManager, get_component_resources_valid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + auto resources = manager->get_component_resources("rclcpp_components"); + EXPECT_EQ(3u, resources.size()); + + EXPECT_EQ("test_rclcpp_components::TestComponentFoo", resources[0].first); + EXPECT_EQ("test_rclcpp_components::TestComponentBar", resources[1].first); + EXPECT_EQ("test_rclcpp_components::TestComponentNoNode", resources[2].first); + + EXPECT_TRUE(rcpputils::fs::path(resources[0].second).exists()); + EXPECT_TRUE(rcpputils::fs::path(resources[1].second).exists()); + EXPECT_TRUE(rcpputils::fs::path(resources[2].second).exists()); +} + +TEST_F(TestComponentManager, create_component_factory_valid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + auto resources = manager->get_component_resources("rclcpp_components"); + EXPECT_EQ(3u, resources.size()); + + // Repeated loading should reuse existing class loader and not throw. + EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]);); + EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]);); + + for (const auto & resource : resources) { + auto factory = manager->create_component_factory(resource); + EXPECT_NE(nullptr, factory); + } +} + +TEST_F(TestComponentManager, create_component_factory_invalid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + // Test invalid library + EXPECT_THROW( + manager->create_component_factory({"foo_class", "invalid_library.so"}), + rclcpp_components::ComponentManagerException); + + // Test valid library with invalid class + auto resources = manager->get_component_resources("rclcpp_components"); + auto factory = manager->create_component_factory({"foo_class", resources[0].second}); + EXPECT_EQ(nullptr, factory); +} diff --git a/rclcpp-eloquent/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp-eloquent/rclcpp_components/test/test_component_manager_api.cpp new file mode 100644 index 0000000..5baac26 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_components/test/test_component_manager_api.cpp @@ -0,0 +1,243 @@ +// Copyright 2019 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 +#include + +#include "composition_interfaces/srv/load_node.hpp" +#include "composition_interfaces/srv/unload_node.hpp" +#include "composition_interfaces/srv/list_nodes.hpp" + +#include "component_manager.hpp" + +using namespace std::chrono_literals; + +class TestComponentManager : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +// TODO(hidmic): split up tests once Node bring up/tear down races +// are solved https://github.com/ros2/rclcpp/issues/863 +TEST_F(TestComponentManager, components_api) +{ + auto exec = std::make_shared(); + auto node = rclcpp::Node::make_shared("test_component_manager"); + auto manager = std::make_shared(exec); + + exec->add_node(manager); + exec->add_node(node); + + auto client = node->create_client( + "/ComponentManager/_container/load_node"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, true); + EXPECT_EQ(result.get()->error_message, ""); + EXPECT_EQ(result.get()->full_node_name, "/test_component_foo"); + EXPECT_EQ(result.get()->unique_id, 1u); + } + + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentBar"; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, true); + EXPECT_EQ(result.get()->error_message, ""); + EXPECT_EQ(result.get()->full_node_name, "/test_component_bar"); + EXPECT_EQ(result.get()->unique_id, 2u); + } + + // Test remapping the node name + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_baz"; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, true); + EXPECT_EQ(result.get()->error_message, ""); + EXPECT_EQ(result.get()->full_node_name, "/test_component_baz"); + EXPECT_EQ(result.get()->unique_id, 3u); + } + + // Test remapping the node namespace + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_namespace = "/ns"; + request->node_name = "test_component_bing"; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, true); + EXPECT_EQ(result.get()->error_message, ""); + EXPECT_EQ(result.get()->full_node_name, "/ns/test_component_bing"); + EXPECT_EQ(result.get()->unique_id, 4u); + } + + { + // Valid package, but invalid class name. + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponent"; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, false); + EXPECT_EQ(result.get()->error_message, "Failed to find class with the requested plugin name."); + EXPECT_EQ(result.get()->full_node_name, ""); + EXPECT_EQ(result.get()->unique_id, 0u); + } + + { + // Invalid package, but valid class name. + auto request = std::make_shared(); + request->package_name = "rclcpp_components_foo"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, false); + EXPECT_EQ(result.get()->error_message, "Could not find requested resource in ament index"); + EXPECT_EQ(result.get()->full_node_name, ""); + EXPECT_EQ(result.get()->unique_id, 0u); + } + + auto node_names = node->get_node_names(); + + auto find_in_nodes = [node_names](std::string name) { + return std::find(node_names.begin(), node_names.end(), name) != node_names.end(); + }; + + EXPECT_TRUE(find_in_nodes("/test_component_foo")); + EXPECT_TRUE(find_in_nodes("/test_component_bar")); + EXPECT_TRUE(find_in_nodes("/test_component_baz")); + EXPECT_TRUE(find_in_nodes("/ns/test_component_bing")); + + { + auto client = node->create_client( + "/ComponentManager/_container/list_nodes"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + auto node_names = result.get()->full_node_names; + auto unique_ids = result.get()->unique_ids; + + EXPECT_EQ(node_names.size(), 4u); + EXPECT_EQ(node_names[0], "/test_component_foo"); + EXPECT_EQ(node_names[1], "/test_component_bar"); + EXPECT_EQ(node_names[2], "/test_component_baz"); + EXPECT_EQ(node_names[3], "/ns/test_component_bing"); + EXPECT_EQ(unique_ids.size(), 4u); + EXPECT_EQ(unique_ids[0], 1u); + EXPECT_EQ(unique_ids[1], 2u); + EXPECT_EQ(unique_ids[2], 3u); + EXPECT_EQ(unique_ids[3], 4u); + } + } + + { + auto client = node->create_client( + "/ComponentManager/_container/unload_node"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + request->unique_id = 1; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, true); + EXPECT_EQ(result.get()->error_message, ""); + } + + { + auto request = std::make_shared(); + request->unique_id = 1; + + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + EXPECT_EQ(result.get()->success, false); + EXPECT_EQ(result.get()->error_message, "No node found with unique_id: 1"); + } + } + + { + auto client = node->create_client( + "/ComponentManager/_container/list_nodes"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + auto result = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS); + auto node_names = result.get()->full_node_names; + auto unique_ids = result.get()->unique_ids; + + EXPECT_EQ(node_names.size(), 3u); + EXPECT_EQ(node_names[0], "/test_component_bar"); + EXPECT_EQ(node_names[1], "/test_component_baz"); + EXPECT_EQ(node_names[2], "/ns/test_component_bing"); + EXPECT_EQ(unique_ids.size(), 3u); + EXPECT_EQ(unique_ids[0], 2u); + EXPECT_EQ(unique_ids[1], 3u); + EXPECT_EQ(unique_ids[2], 4u); + } + } +} diff --git a/rclcpp-eloquent/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp-eloquent/rclcpp_lifecycle/CHANGELOG.rst new file mode 100644 index 0000000..baaf0cf --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/CHANGELOG.rst @@ -0,0 +1,108 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp_lifecycle +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + +0.8.5 (2020-12-04) +------------------ + +0.8.4 (2020-01-17) +------------------ + +0.8.3 (2019-11-19) +------------------ + +0.8.2 (2019-11-18) +------------------ + +0.8.1 (2019-10-23) +------------------ +* New Intra-Process Communication (`#778 `_) +* Contributors: Alberto Soragna + +0.8.0 (2019-09-26) +------------------ +* clean up publisher and subscription creation logic (`#867 `_) +* reset error message before setting a new one, embed the original one (`#854 `_) +* remove features and related code which were deprecated in dashing (`#852 `_) +* Fix typo in deprecated warning. (`#848 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* Fixe error messages not printing to terminal (`#777 `_) +* Add default value to options in LifecycleNode construnctor. Update API documentation. (`#775 `_) +* Contributors: Dan Rose, Dirk Thomas, Esteve Fernandez, Luca Della Vedova, William Woodall, Yathartha Tuladhar + +0.7.5 (2019-05-30) +------------------ + +0.7.4 (2019-05-29) +------------------ +* Rename parameter options (`#745 `_) +* Contributors: William Woodall + +0.7.3 (2019-05-20) +------------------ +* Added missing template functionality to lifecycle_node. (`#707 `_) +* Contributors: Michael Jeronimo + +0.7.2 (2019-05-08) +------------------ +* Added new way to specify QoS settings for publishers and subscriptions. (`#713 `_) +* Deprecated ``shared_ptr`` and raw pointer versions of ``Publisher::publish()``. (`#709 `_) +* Implemented API to set callbacks for liveliness and deadline QoS events for publishers and subscriptions. (`#695 `_) +* Changed the ``IntraProcessManager`` to be capable of storing ``shared_ptr`` in addition to ``unique_ptr``. (`#690 `_) +* Contributors: M. M, William Woodall, ivanpauno + +0.7.1 (2019-04-26) +------------------ +* Added read only parameters. (`#495 `_) +* Contributors: Shane Loretz, William Woodall + +0.7.0 (2019-04-14) +------------------ +* Fixed linter errors in rclcpp_lifecycle. (`#672 `_) +* Added parameter-related templates to LifecycleNode. (`#645 `_) +* Fixed use_sim_time issue on LifeCycleNode. (`#651 `_) +* Updated to use ament_target_dependencies where possible. (`#659 `_) +* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 `_) +* Added a method to the LifecycleNode class to get the logging interface. (`#652 `_) +* Set Parameter Event Publisher settings `#591 `_ (`#614 `_) +* Replaced node constructor arguments with NodeOptions. (`#622 `_) +* Removed dependency on rclpy. (`#626 `_) +* Contributors: Emerson Knapp, Karsten Knese, Michael Carroll, Michael Jeronimo, Vinnam Kim, William Woodall, ivanpauno, rarvolt + +0.6.2 (2018-12-13) +------------------ + +0.6.1 (2018-12-07) +------------------ +* Added node path and time stamp to parameter event message (`#584 `_) +* Refactored init to allow for non-global init (`#587 `_) +* Add class Waitable (`#589 `_) +* Contributors: Dirk Thomas, Jacob Perron, William Woodall, bpwilcox + +0.6.0 (2018-11-19) +------------------ +* Updated to use new error handling API from rcutils (`#577 `_) +* Deleted TRANSITION_SHUTDOWN (`#576 `_) +* Added a warning when publishing if publisher is not active (`#574 `_) +* Added SMART_PTRS_DEF to LifecyclePublisher (`#569 `_) +* Added service for transition graph (`#555 `_) +* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 `_) +* Fixed and improved documentation (`#546 `_) +* Removed unneeded dependency on std_msgs (`#513 `_) +* Removed use of uninitialized CMake var (`#511 `_) +* Added get_node_names API from node. (`#508 `_) +* Fixed rosidl dependencies (`#507 `_) +* Contributors: Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Mikael Arguedas, Sriram Raghunathan, William Woodall, cho3 + +0.5.0 (2018-06-25) +------------------ +* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 `_) +* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 `_) +* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 `_) +* Fixed a bug that occurred when mixing ``std::shared_ptr`` and ``std::bind``. (`#470 `_) +* Added ability to pass command line arguments to the Node constructor. (`#461 `_) +* Changed library export order for static linking. (`#446 `_) +* Now depends on ``ament_cmake_ros``. (`#444 `_) +* Updaed code to use logging macros rather than ``fprintf()``. (`#439 `_) +* Contributors: Dirk Thomas, Guillaume Autran, Karsten Knese, Michael Carroll, Mikael Arguedas, Shane Loretz, dhood diff --git a/rclcpp-eloquent/rclcpp_lifecycle/CMakeLists.txt b/rclcpp-eloquent/rclcpp_lifecycle/CMakeLists.txt new file mode 100644 index 0000000..373667a --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp_lifecycle) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcl_lifecycle REQUIRED) +find_package(rosidl_typesupport_cpp REQUIRED) +find_package(lifecycle_msgs REQUIRED) + +include_directories(include) + +### CPP High level library +add_library(rclcpp_lifecycle + src/lifecycle_node.cpp + src/node_interfaces/lifecycle_node_interface.cpp + src/state.cpp + src/transition.cpp +) +# specific order: dependents before dependencies +ament_target_dependencies(rclcpp_lifecycle + "rclcpp" + "rcl_lifecycle" + "lifecycle_msgs" + "rosidl_typesupport_cpp" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(rclcpp_lifecycle PRIVATE "RCLCPP_LIFECYCLE_BUILDING_DLL") + +install(TARGETS + rclcpp_lifecycle + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp) + if(TARGET test_lifecycle_node) + ament_target_dependencies(test_lifecycle_node + "rcl_lifecycle" + "rclcpp" + ) + target_link_libraries(test_lifecycle_node ${PROJECT_NAME}) + endif() + ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) + if(TARGET test_state_machine_info) + ament_target_dependencies(test_state_machine_info + "rcl_lifecycle" + "rclcpp" + ) + target_link_libraries(test_state_machine_info ${PROJECT_NAME}) + endif() + ament_add_gtest(test_register_custom_callbacks test/test_register_custom_callbacks.cpp) + if(TARGET test_register_custom_callbacks) + ament_target_dependencies(test_register_custom_callbacks + "rcl_lifecycle" + "rclcpp" + ) + target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME}) + endif() + ament_add_gtest(test_callback_exceptions test/test_callback_exceptions.cpp) + if(TARGET test_callback_exceptions) + ament_target_dependencies(test_callback_exceptions + "rcl_lifecycle" + "rclcpp" + ) + target_link_libraries(test_callback_exceptions ${PROJECT_NAME}) + endif() + ament_add_gtest(test_state_wrapper test/test_state_wrapper.cpp) + if(TARGET test_state_wrapper) + ament_target_dependencies(test_state_wrapper + "rcl_lifecycle" + "rclcpp" + ) + target_link_libraries(test_state_wrapper ${PROJECT_NAME}) + endif() + ament_add_gtest(test_transition_wrapper test/test_transition_wrapper.cpp) + if(TARGET test_transition_wrapper) + ament_target_dependencies(test_transition_wrapper + "rcl_lifecycle" + "rclcpp" + ) + target_link_libraries(test_transition_wrapper ${PROJECT_NAME}) + endif() +endif() + +# specific order: dependents before dependencies +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rcl_lifecycle) +ament_export_dependencies(lifecycle_msgs) +ament_package() + +install(DIRECTORY include/ + DESTINATION include) diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp new file mode 100644 index 0000000..9d9fa7e --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -0,0 +1,668 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/node.h" + +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/timer.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/transition.hpp" +#include "rclcpp_lifecycle/visibility_control.h" + +namespace rclcpp_lifecycle +{ + +// include these here to work around an esoteric Windows error where the namespace +// cannot be used in the function declaration below without getting an error like: +// 'rclcpp::SubscriptionOptionsWithAllocator': +// no appropriate default constructor available +template +using PublisherOptionsWithAllocator = rclcpp::PublisherOptionsWithAllocator; +template +using SubscriptionOptionsWithAllocator = rclcpp::SubscriptionOptionsWithAllocator; + +template +PublisherOptionsWithAllocator +create_default_publisher_options() +{ + return rclcpp::PublisherOptionsWithAllocator(); +} + +template +SubscriptionOptionsWithAllocator +create_default_subscription_options() +{ + return rclcpp::SubscriptionOptionsWithAllocator(); +} + +/// LifecycleNode for creating lifecycle components +/** + * has lifecycle nodeinterface for configuring this node. + */ +class LifecycleNode : public node_interfaces::LifecycleNodeInterface, + public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(LifecycleNode) + + /// Create a new lifecycle node with the specified name. + /** + * \param[in] node_name Name of the node. + * \param[in] namespace_ Namespace of the node. + * \param[in] options Additional options to control creation of the node. + */ + RCLCPP_LIFECYCLE_PUBLIC + explicit LifecycleNode( + const std::string & node_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// Create a node based on the node name and a rclcpp::Context. + /** + * \param[in] node_name Name of the node. + * \param[in] namespace_ Namespace of the node. + * \param[in] options Additional options to control creation of the node. + */ + RCLCPP_LIFECYCLE_PUBLIC + LifecycleNode( + const std::string & node_name, + const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + RCLCPP_LIFECYCLE_PUBLIC + virtual ~LifecycleNode(); + + /// Get the name of the node. + // \return The name of the node. + RCLCPP_LIFECYCLE_PUBLIC + const char * + get_name() const; + + /// Get the namespace of the node. + // \return The namespace of the node. + RCLCPP_LIFECYCLE_PUBLIC + const char * + get_namespace() const; + + /// Get the logger of the node. + /** \return The logger of the node. */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::Logger + get_logger() const; + + /// Create and return a callback group. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + + /// Return the list of callback groups in the node. + RCLCPP_LIFECYCLE_PUBLIC + const std::vector & + get_callback_groups() const; + + /// Create and return a Publisher. + /** + * \param[in] topic_name The topic for this publisher to publish on. + * \param[in] qos The Quality of Service settings for this publisher. + * \param[in] options The publisher options for this publisher. + * \return Shared pointer to the created lifecycle publisher. + */ + template> + std::shared_ptr> + create_publisher( + const std::string & topic_name, + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options = ( + create_default_publisher_options() + ) + ); + + /// Create and return a Subscription. + /** + * \param[in] topic_name The topic to subscribe on. + * \param[in] callback The user-defined callback function. + * \param[in] qos The quality of service for this subscription. + * \param[in] options The subscription options for this subscription. + * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. + * \return Shared pointer to the created subscription. + */ + template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + AllocatorT + >> + std::shared_ptr + create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const SubscriptionOptionsWithAllocator & options = + create_default_subscription_options(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) + ); + + /// Create a timer. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::WallTimer::SharedPtr + create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /* Create and return a Client. */ + template + typename rclcpp::Client::SharedPtr + create_client( + const std::string & service_name, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /* Create and return a Service. */ + template + typename rclcpp::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + + /// Declare and initialize a parameter, return the effective value. + /** + * \sa rclcpp::Node::declare_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()); + + /// Declare and initialize a parameter with a type. + /** + * \sa rclcpp::Node::declare_parameter + */ + template + auto + declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()); + + /// Declare and initialize several parameters with the same namespace and type. + /** + * \sa rclcpp::Node::declare_parameters + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map & parameters); + + /// Declare and initialize several parameters with the same namespace and type. + /** + * \sa rclcpp::Node::declare_parameters + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters); + + /// Undeclare a previously declared parameter. + /** + * \sa rclcpp::Node::undeclare_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + void + undeclare_parameter(const std::string & name); + + /// Return true if a given parameter is declared. + /** + * \sa rclcpp::Node::has_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + bool + has_parameter(const std::string & name) const; + + /// Set a single parameter. + /** + * \sa rclcpp::Node::set_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameter(const rclcpp::Parameter & parameter); + + /// Set one or more parameters, one at a time. + /** + * \sa rclcpp::Node::set_parameters + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + set_parameters(const std::vector & parameters); + + /// Set one or more parameters, all at once. + /** + * \sa rclcpp::Node::set_parameters_atomically + */ + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically(const std::vector & parameters); + + /// Return the parameter by the given name. + /** + * \sa rclcpp::Node::get_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::Parameter + get_parameter(const std::string & name) const; + + /// Get the value of a parameter by the given name, and return true if it was set. + /** + * \sa rclcpp::Node::get_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + bool + get_parameter( + const std::string & name, + rclcpp::Parameter & parameter) const; + + /// Get the value of a parameter by the given name, and return true if it was set. + /** + * \sa rclcpp::Node::get_parameter + */ + template + bool + get_parameter(const std::string & name, ParameterT & parameter) const; + + /// Get the parameter value, or the "alternative_value" if not set, and assign it to "parameter". + /** + * \sa rclcpp::Node::get_parameter_or + */ + template + bool + get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const; + + /// Return the parameters by the given parameter names. + /** + * \sa rclcpp::Node::get_parameters + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_parameters(const std::vector & names) const; + + /// Get the parameter values for all parameters that have a given prefix. + /** + * \sa rclcpp::Node::get_parameters + */ + template + bool + get_parameters( + const std::string & prefix, + std::map & values) const; + + /// Return the parameter descriptor for the given parameter name. + /** + * \sa rclcpp::Node::describe_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::ParameterDescriptor + describe_parameter(const std::string & name) const; + + /// Return a vector of parameter descriptors, one for each of the given names. + /** + * \sa rclcpp::Node::describe_parameters + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + describe_parameters(const std::vector & names) const; + + /// Return a vector of parameter types, one for each of the given names. + /** + * \sa rclcpp::Node::get_parameter_types + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_parameter_types(const std::vector & names) const; + + /// Return a list of parameters with any of the given prefixes, up to the given depth. + /** + * \sa rclcpp::Node::list_parameters + */ + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const; + + using OnParametersSetCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + + /// Register a callback to be called anytime a parameter is about to be changed. + /** + * \sa rclcpp::Node::set_on_parameters_set_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType + set_on_parameters_set_callback( + rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback); + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_node_names() const; + + RCLCPP_LIFECYCLE_PUBLIC + std::map> + get_topic_names_and_types(bool no_demangle = false) const; + + RCLCPP_LIFECYCLE_PUBLIC + std::map> + get_service_names_and_types() const; + + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_publishers(const std::string & topic_name) const; + + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_subscribers(const std::string & topic_name) const; + + /// Return a graph event, which will be set anytime a graph change occurs. + /* The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the load just let it go + * out of scope. + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::Event::SharedPtr + get_graph_event(); + + /// Wait for a graph event to occur by waiting on an Event to become set. + /* The given Event must be acquire through the get_graph_event() method. + * + * \throws InvalidEventError if the given event is nullptr + * \throws EventNotRegisteredError if the given event was not acquired with + * get_graph_event(). + */ + RCLCPP_LIFECYCLE_PUBLIC + void + wait_for_graph_change( + rclcpp::Event::SharedPtr event, + std::chrono::nanoseconds timeout); + + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::Clock::SharedPtr + get_clock(); + + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::Time + now(); + + /// Return the Node's internal NodeBaseInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface(); + + /// Return the Node's internal NodeClockInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeClockInterface::SharedPtr + get_node_clock_interface(); + + /// Return the Node's internal NodeGraphInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr + get_node_graph_interface(); + + /// Return the Node's internal NodeLoggingInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface(); + + /// Return the Node's internal NodeTimersInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr + get_node_timers_interface(); + + /// Return the Node's internal NodeTopicsInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface(); + + /// Return the Node's internal NodeServicesInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface(); + + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + get_node_parameters_interface(); + + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface(); + + /// Return the Node's internal NodeWaitablesInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface(); + + /// Return the NodeOptions used when creating this node. + RCLCPP_LIFECYCLE_PUBLIC + const rclcpp::NodeOptions & + get_node_options() const; + + // + // LIFECYCLE COMPONENTS + // + RCLCPP_LIFECYCLE_PUBLIC + const State & + get_current_state(); + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_available_states(); + + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_available_transitions(); + + /// trigger the specified transition + /* + * return the new state after this transition + */ + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition(const Transition & transition); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition( + const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition(uint8_t transition_id); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition( + uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + configure(); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + configure(LifecycleNodeInterface::CallbackReturn & cb_return_code); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + cleanup(); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + activate(); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + activate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + deactivate(); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + shutdown(); + + RCLCPP_LIFECYCLE_PUBLIC + const State & + shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code); + + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_configure(std::function fcn); + + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_cleanup(std::function fcn); + + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_shutdown(std::function fcn); + + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_activate(std::function fcn); + + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_deactivate(std::function fcn); + + RCLCPP_LIFECYCLE_PUBLIC + bool + register_on_error(std::function fcn); + +protected: + RCLCPP_LIFECYCLE_PUBLIC + void + add_publisher_handle(std::shared_ptr pub); + + RCLCPP_LIFECYCLE_PUBLIC + void + add_timer_handle(std::shared_ptr timer); + +private: + RCLCPP_DISABLE_COPY(LifecycleNode) + + RCLCPP_LIFECYCLE_PUBLIC + bool + group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; + + const rclcpp::NodeOptions node_options_; + + class LifecycleNodeInterfaceImpl; + std::unique_ptr impl_; +}; + +} // namespace rclcpp_lifecycle + +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +// Template implementations +#include "rclcpp_lifecycle/lifecycle_node_impl.hpp" +#endif + +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp new file mode 100644 index 0000000..c090319 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -0,0 +1,235 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_service.hpp" +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/type_support_decl.hpp" + +#include "lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/visibility_control.h" + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace rclcpp_lifecycle +{ + +template +std::shared_ptr> +LifecycleNode::create_publisher( + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + using PublisherT = rclcpp_lifecycle::LifecyclePublisher; + return rclcpp::create_publisher( + *this, + topic_name, + qos, + options); +} + +// TODO(karsten1987): Create LifecycleSubscriber +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename CallbackMessageT, + typename SubscriptionT, + typename MessageMemoryStrategyT> +std::shared_ptr +LifecycleNode::create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) +{ + return rclcpp::create_subscription( + *this, + topic_name, + qos, + std::forward(callback), + options, + msg_mem_strat); +} + +template +typename rclcpp::WallTimer::SharedPtr +LifecycleNode::create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + auto timer = rclcpp::WallTimer::make_shared( + std::chrono::duration_cast(period), + std::move(callback), this->node_base_->get_context()); + node_timers_->add_timer(timer, group); + return timer; +} + +template +typename rclcpp::Client::SharedPtr +LifecycleNode::create_client( + const std::string & service_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos_profile; + + using rclcpp::Client; + using rclcpp::ClientBase; + + auto cli = Client::make_shared( + node_base_.get(), + node_graph_, + service_name, + options); + + auto cli_base_ptr = std::dynamic_pointer_cast(cli); + node_services_->add_client(cli_base_ptr, group); + return cli; +} + +template +typename rclcpp::Service::SharedPtr +LifecycleNode::create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_service( + node_base_, node_services_, + service_name, std::forward(callback), qos_profile, group); +} + +template +auto +LifecycleNode::declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) +{ + return this->declare_parameter( + name, + rclcpp::ParameterValue(default_value), + parameter_descriptor + ).get(); +} + +template +std::vector +LifecycleNode::declare_parameters( + const std::string & namespace_, + const std::map & parameters) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace](auto element) { + return this->declare_parameter(normalized_namespace + element.first, element.second); + } + ); + return result; +} + +template +std::vector +LifecycleNode::declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace](auto element) { + return static_cast( + this->declare_parameter( + normalized_namespace + element.first, + element.second.first, + element.second.second) + ); + } + ); + return result; +} + +template +bool +LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) const +{ + rclcpp::Parameter param(name, parameter); + bool result = get_parameter(name, param); + parameter = param.get_value(); + + return result; +} + +// this is a partially-specialized version of get_parameter above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +bool +LifecycleNode::get_parameters( + const std::string & prefix, + std::map & values) const +{ + std::map params; + bool result = node_parameters_->get_parameters_by_prefix(prefix, params); + if (result) { + for (const auto & param : params) { + values[param.first] = param.second.get_value(); + } + } + + return result; +} + +template +bool +LifecycleNode::get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const +{ + bool got_parameter = get_parameter(name, value); + if (!got_parameter) { + value = alternative_value; + } + return got_parameter; +} + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp new file mode 100644 index 0000000..2f94dc7 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -0,0 +1,138 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ + +#include +#include +#include + +#include "rclcpp/publisher.hpp" + +#include "rclcpp/logging.hpp" + +namespace rclcpp_lifecycle +{ +/// base class with only +/** + * pure virtual functions. A managed + * node can then deactivate or activate + * the publishing. + * It is more a convenient interface class + * than a necessary base class. + */ +class LifecyclePublisherInterface +{ +public: + virtual void on_activate() = 0; + virtual void on_deactivate() = 0; + virtual bool is_activated() = 0; +}; + +/// brief child class of rclcpp Publisher class. +/** + * Overrides all publisher functions to check for enabled/disabled state. + */ +template> +class LifecyclePublisher : public LifecyclePublisherInterface, + public rclcpp::Publisher +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(LifecyclePublisher) + + using MessageAllocTraits = rclcpp::allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = rclcpp::allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; + + LifecyclePublisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + : rclcpp::Publisher(node_base, topic, qos, options), + enabled_(false), + logger_(rclcpp::get_logger("LifecyclePublisher")) + { + } + + ~LifecyclePublisher() {} + + /// LifecyclePublisher publish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish(std::unique_ptr msg) + { + if (!enabled_) { + RCLCPP_WARN( + logger_, + "Trying to publish message on the topic '%s', but the publisher is not activated", + this->get_topic_name()); + + return; + } + rclcpp::Publisher::publish(std::move(msg)); + } + + /// LifecyclePublisher publish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish(const MessageT & msg) + { + if (!enabled_) { + RCLCPP_WARN( + logger_, + "Trying to publish message on the topic '%s', but the publisher is not activated", + this->get_topic_name()); + + return; + } + rclcpp::Publisher::publish(msg); + } + + virtual void + on_activate() + { + enabled_ = true; + } + + virtual void + on_deactivate() + { + enabled_ = false; + } + + virtual bool + is_activated() + { + return enabled_; + } + +private: + bool enabled_ = false; + rclcpp::Logger logger_; +}; + +} // namespace rclcpp_lifecycle + +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_PUBLISHER_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp new file mode 100644 index 0000000..86ebc22 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -0,0 +1,107 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ +#define RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ + +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/visibility_control.h" + +namespace rclcpp_lifecycle +{ +namespace node_interfaces +{ +/// Interface class for a managed node. +/** Virtual functions as defined in + * http://design.ros2.org/articles/node_lifecycle.html + * + * If the callback function returns successfully, + * the specified transition is completed. + * If the callback function fails or throws an + * uncaught exception, the on_error function is + * called. + * By default, all functions remain optional to overwrite + * and return true. Except the on_error function, which + * returns false and thus goes to shutdown/finalize state. + */ +class LifecycleNodeInterface +{ +protected: + RCLCPP_LIFECYCLE_PUBLIC + LifecycleNodeInterface() {} + +public: + enum class CallbackReturn : uint8_t + { + SUCCESS = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, + FAILURE = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE, + ERROR = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR + }; + + /// Callback function for configure transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual CallbackReturn + on_configure(const State & previous_state); + + /// Callback function for cleanup transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual CallbackReturn + on_cleanup(const State & previous_state); + + /// Callback function for shutdown transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual CallbackReturn + on_shutdown(const State & previous_state); + + /// Callback function for activate transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual CallbackReturn + on_activate(const State & previous_state); + + /// Callback function for deactivate transition + /* + * \return true by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual CallbackReturn + on_deactivate(const State & previous_state); + + /// Callback function for errorneous transition + /* + * \return false by default + */ + RCLCPP_LIFECYCLE_PUBLIC + virtual CallbackReturn + on_error(const State & previous_state); +}; + +} // namespace node_interfaces +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp new file mode 100644 index 0000000..eecbf43 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -0,0 +1,77 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__STATE_HPP_ +#define RCLCPP_LIFECYCLE__STATE_HPP_ + +#include + +#include "rclcpp_lifecycle/visibility_control.h" + +#include "rcutils/allocator.h" + +// forward declare rcl_state_t +typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t; + +namespace rclcpp_lifecycle +{ + +class State +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + explicit State(rcutils_allocator_t allocator = rcutils_get_default_allocator()); + + RCLCPP_LIFECYCLE_PUBLIC + State( + uint8_t id, + const std::string & label, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); + + RCLCPP_LIFECYCLE_PUBLIC + explicit State( + const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); + + RCLCPP_LIFECYCLE_PUBLIC + State(const State & rhs); + + RCLCPP_LIFECYCLE_PUBLIC + virtual ~State(); + + RCLCPP_LIFECYCLE_PUBLIC + State & operator=(const State & rhs); + + RCLCPP_LIFECYCLE_PUBLIC + uint8_t + id() const; + + RCLCPP_LIFECYCLE_PUBLIC + std::string + label() const; + +protected: + RCLCPP_LIFECYCLE_PUBLIC + void + reset(); + + rcutils_allocator_t allocator_; + + bool owns_rcl_state_handle_; + + rcl_lifecycle_state_t * state_handle_; +}; + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__STATE_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp new file mode 100644 index 0000000..a566a22 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -0,0 +1,92 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__TRANSITION_HPP_ +#define RCLCPP_LIFECYCLE__TRANSITION_HPP_ + +#include + +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/visibility_control.h" + +#include "rcutils/allocator.h" + +// forward declare rcl_transition_t +typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; + +namespace rclcpp_lifecycle +{ + +class Transition +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + Transition() = delete; + + RCLCPP_LIFECYCLE_PUBLIC + explicit Transition( + uint8_t id, + const std::string & label = "", + rcutils_allocator_t allocator = rcutils_get_default_allocator()); + + RCLCPP_LIFECYCLE_PUBLIC + Transition( + uint8_t id, const std::string & label, + State && start, State && goal, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); + + RCLCPP_LIFECYCLE_PUBLIC + explicit Transition( + const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, + rcutils_allocator_t allocator = rcutils_get_default_allocator()); + + RCLCPP_LIFECYCLE_PUBLIC + Transition(const Transition & rhs); + + RCLCPP_LIFECYCLE_PUBLIC + virtual ~Transition(); + + RCLCPP_LIFECYCLE_PUBLIC + Transition & operator=(const Transition & rhs); + + RCLCPP_LIFECYCLE_PUBLIC + uint8_t + id() const; + + RCLCPP_LIFECYCLE_PUBLIC + std::string + label() const; + + RCLCPP_LIFECYCLE_PUBLIC + State + start_state() const; + + RCLCPP_LIFECYCLE_PUBLIC + State + goal_state() const; + +protected: + RCLCPP_LIFECYCLE_PUBLIC + void + reset(); + + rcutils_allocator_t allocator_; + + bool owns_rcl_transition_handle_; + + rcl_lifecycle_transition_t * transition_handle_; +}; + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__TRANSITION_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp new file mode 100644 index 0000000..1a5313a --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/type_traits/is_manageable_node.hpp @@ -0,0 +1,62 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ +#define RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ + +#include +#include + +template +struct has_on_activate +{ + static constexpr bool value = false; +}; + +template +struct has_on_activate< + T, + typename std::enable_if< + std::is_same().on_activate())>::value>::type> +{ + static constexpr bool value = true; +}; + +template +struct has_on_deactivate +{ + static constexpr bool value = false; +}; + +template +struct has_on_deactivate< + T, + typename std::enable_if< + std::is_same().on_deactivate())>::value>::type> +{ + static constexpr bool value = true; +}; + +template +struct is_manageable_node : std::false_type +{}; + +template +struct is_manageable_node< + T, + typename std::enable_if< + has_on_activate::value && has_on_deactivate::value>::type>: std::true_type +{}; + +#endif // RCLCPP_LIFECYCLE__TYPE_TRAITS__IS_MANAGEABLE_NODE_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h new file mode 100644 index 0000000..b2a8327 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/include/rclcpp_lifecycle/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ +#define RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCLCPP_LIFECYCLE_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_LIFECYCLE_IMPORT __attribute__ ((dllimport)) + #else + #define RCLCPP_LIFECYCLE_EXPORT __declspec(dllexport) + #define RCLCPP_LIFECYCLE_IMPORT __declspec(dllimport) + #endif + #ifdef RCLCPP_LIFECYCLE_BUILDING_DLL + #define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_EXPORT + #else + #define RCLCPP_LIFECYCLE_PUBLIC RCLCPP_LIFECYCLE_IMPORT + #endif + #define RCLCPP_LIFECYCLE_PUBLIC_TYPE RCLCPP_LIFECYCLE_PUBLIC + #define RCLCPP_LIFECYCLE_LOCAL +#else + #define RCLCPP_LIFECYCLE_EXPORT __attribute__ ((visibility("default"))) + #define RCLCPP_LIFECYCLE_IMPORT + #if __GNUC__ >= 4 + #define RCLCPP_LIFECYCLE_PUBLIC __attribute__ ((visibility("default"))) + #define RCLCPP_LIFECYCLE_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCLCPP_LIFECYCLE_PUBLIC + #define RCLCPP_LIFECYCLE_LOCAL + #endif + #define RCLCPP_LIFECYCLE_PUBLIC_TYPE +#endif + +#endif // RCLCPP_LIFECYCLE__VISIBILITY_CONTROL_H_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/package.xml b/rclcpp-eloquent/rclcpp_lifecycle/package.xml new file mode 100644 index 0000000..57dae0e --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/package.xml @@ -0,0 +1,31 @@ + + + + rclcpp_lifecycle + 0.8.5 + Package containing a prototype for lifecycle implementation + Karsten Knese + Apache License 2.0 + + ament_cmake_ros + + lifecycle_msgs + rclcpp + rcl_lifecycle + rmw_implementation + rosidl_typesupport_cpp + + lifecycle_msgs + rclcpp + rcl_lifecycle + rmw_implementation + rosidl_typesupport_cpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node.cpp new file mode 100644 index 0000000..90e7f1c --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -0,0 +1,567 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/graph_listener.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_clock.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/node_interfaces/node_logging.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" +#include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_time_source.hpp" +#include "rclcpp/node_interfaces/node_timers.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" +#include "rclcpp/parameter_service.hpp" + +#include "lifecycle_node_interface_impl.hpp" // implementation + +namespace rclcpp_lifecycle +{ + +LifecycleNode::LifecycleNode( + const std::string & node_name, + const rclcpp::NodeOptions & options) +: LifecycleNode( + node_name, + "", + options) +{} + +LifecycleNode::LifecycleNode( + const std::string & node_name, + const std::string & namespace_, + const rclcpp::NodeOptions & options) +: node_base_(new rclcpp::node_interfaces::NodeBase( + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms())), + node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), + node_clock_(new rclcpp::node_interfaces::NodeClock( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_ + )), + node_parameters_(new rclcpp::node_interfaces::NodeParameters( + node_base_, + node_logging_, + node_topics_, + node_services_, + node_clock_, + options.parameter_overrides(), + options.start_parameter_services(), + options.start_parameter_event_publisher(), + options.parameter_event_qos(), + options.parameter_event_publisher_options(), + options.allow_undeclared_parameters(), + options.automatically_declare_parameters_from_overrides() + )), + node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_ + )), + node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), + node_options_(options), + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) +{ + impl_->init(); + + register_on_configure( + std::bind( + &LifecycleNodeInterface::on_configure, this, + std::placeholders::_1)); + register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this, std::placeholders::_1)); + register_on_shutdown( + std::bind( + &LifecycleNodeInterface::on_shutdown, this, + std::placeholders::_1)); + register_on_activate( + std::bind( + &LifecycleNodeInterface::on_activate, this, + std::placeholders::_1)); + register_on_deactivate( + std::bind( + &LifecycleNodeInterface::on_deactivate, this, + std::placeholders::_1)); + register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1)); +} + +LifecycleNode::~LifecycleNode() +{} + +const char * +LifecycleNode::get_name() const +{ + return node_base_->get_name(); +} + +const char * +LifecycleNode::get_namespace() const +{ + return node_base_->get_namespace(); +} + +rclcpp::Logger +LifecycleNode::get_logger() const +{ + return node_logging_->get_logger(); +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +LifecycleNode::create_callback_group( + rclcpp::callback_group::CallbackGroupType group_type) +{ + return node_base_->create_callback_group(group_type); +} + +const rclcpp::ParameterValue & +LifecycleNode::declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) +{ + return this->node_parameters_->declare_parameter(name, default_value, parameter_descriptor); +} + +void +LifecycleNode::undeclare_parameter(const std::string & name) +{ + this->node_parameters_->undeclare_parameter(name); +} + +bool +LifecycleNode::has_parameter(const std::string & name) const +{ + return this->node_parameters_->has_parameter(name); +} + +rcl_interfaces::msg::SetParametersResult +LifecycleNode::set_parameter(const rclcpp::Parameter & parameter) +{ + return this->set_parameters_atomically({parameter}); +} + +bool +LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + return node_base_->callback_group_in_node(group); +} + +std::vector +LifecycleNode::set_parameters( + const std::vector & parameters) +{ + return node_parameters_->set_parameters(parameters); +} + +rcl_interfaces::msg::SetParametersResult +LifecycleNode::set_parameters_atomically( + const std::vector & parameters) +{ + return node_parameters_->set_parameters_atomically(parameters); +} + +std::vector +LifecycleNode::get_parameters( + const std::vector & names) const +{ + return node_parameters_->get_parameters(names); +} + +rclcpp::Parameter +LifecycleNode::get_parameter(const std::string & name) const +{ + return node_parameters_->get_parameter(name); +} + +bool +LifecycleNode::get_parameter( + const std::string & name, + rclcpp::Parameter & parameter) const +{ + return node_parameters_->get_parameter(name, parameter); +} + +rcl_interfaces::msg::ParameterDescriptor +LifecycleNode::describe_parameter(const std::string & name) const +{ + auto result = node_parameters_->describe_parameters({name}); + if (0 == result.size()) { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } + if (result.size() > 1) { + throw std::runtime_error("number of described parameters unexpectedly more than one"); + } + return result.front(); +} + +std::vector +LifecycleNode::describe_parameters( + const std::vector & names) const +{ + return node_parameters_->describe_parameters(names); +} + +std::vector +LifecycleNode::get_parameter_types( + const std::vector & names) const +{ + return node_parameters_->get_parameter_types(names); +} + +rcl_interfaces::msg::ListParametersResult +LifecycleNode::list_parameters( + const std::vector & prefixes, uint64_t depth) const +{ + return node_parameters_->list_parameters(prefixes, depth); +} + +rclcpp::Node::OnParametersSetCallbackType +LifecycleNode::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback) +{ + return node_parameters_->set_on_parameters_set_callback(callback); +} + +std::vector +LifecycleNode::get_node_names() const +{ + return node_graph_->get_node_names(); +} + +std::map> +LifecycleNode::get_topic_names_and_types(bool no_demangle) const +{ + return node_graph_->get_topic_names_and_types(no_demangle); +} + +std::map> +LifecycleNode::get_service_names_and_types() const +{ + return node_graph_->get_service_names_and_types(); +} + +size_t +LifecycleNode::count_publishers(const std::string & topic_name) const +{ + return node_graph_->count_publishers(topic_name); +} + +size_t +LifecycleNode::count_subscribers(const std::string & topic_name) const +{ + return node_graph_->count_subscribers(topic_name); +} + +const std::vector & +LifecycleNode::get_callback_groups() const +{ + return node_base_->get_callback_groups(); +} + +rclcpp::Event::SharedPtr +LifecycleNode::get_graph_event() +{ + return node_graph_->get_graph_event(); +} + +void +LifecycleNode::wait_for_graph_change( + rclcpp::Event::SharedPtr event, + std::chrono::nanoseconds timeout) +{ + node_graph_->wait_for_graph_change(event, timeout); +} + +rclcpp::Clock::SharedPtr +LifecycleNode::get_clock() +{ + return node_clock_->get_clock(); +} + +rclcpp::Time +LifecycleNode::now() +{ + return node_clock_->get_clock()->now(); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +LifecycleNode::get_node_base_interface() +{ + return node_base_; +} + +rclcpp::node_interfaces::NodeClockInterface::SharedPtr +LifecycleNode::get_node_clock_interface() +{ + return node_clock_; +} + +rclcpp::node_interfaces::NodeGraphInterface::SharedPtr +LifecycleNode::get_node_graph_interface() +{ + return node_graph_; +} + +rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr +LifecycleNode::get_node_logging_interface() +{ + return node_logging_; +} + +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +LifecycleNode::get_node_time_source_interface() +{ + return node_time_source_; +} + +rclcpp::node_interfaces::NodeTimersInterface::SharedPtr +LifecycleNode::get_node_timers_interface() +{ + return node_timers_; +} + +rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr +LifecycleNode::get_node_topics_interface() +{ + return node_topics_; +} + +rclcpp::node_interfaces::NodeServicesInterface::SharedPtr +LifecycleNode::get_node_services_interface() +{ + return node_services_; +} + +rclcpp::node_interfaces::NodeParametersInterface::SharedPtr +LifecycleNode::get_node_parameters_interface() +{ + return node_parameters_; +} + +rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr +LifecycleNode::get_node_waitables_interface() +{ + return node_waitables_; +} + +const rclcpp::NodeOptions & +LifecycleNode::get_node_options() const +{ + return node_options_; +} + +//// +bool +LifecycleNode::register_on_configure( + std::function fcn) +{ + return impl_->register_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); +} + +bool +LifecycleNode::register_on_cleanup( + std::function fcn) +{ + return impl_->register_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); +} + +bool +LifecycleNode::register_on_shutdown( + std::function fcn) +{ + return impl_->register_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); +} + +bool +LifecycleNode::register_on_activate( + std::function fcn) +{ + return impl_->register_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); +} + +bool +LifecycleNode::register_on_deactivate( + std::function fcn) +{ + return impl_->register_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); +} + +bool +LifecycleNode::register_on_error( + std::function fcn) +{ + return impl_->register_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); +} + +const State & +LifecycleNode::get_current_state() +{ + return impl_->get_current_state(); +} + +std::vector +LifecycleNode::get_available_states() +{ + return impl_->get_available_states(); +} + +std::vector +LifecycleNode::get_available_transitions() +{ + return impl_->get_available_transitions(); +} + +const State & +LifecycleNode::trigger_transition(const Transition & transition) +{ + return trigger_transition(transition.id()); +} + +const State & +LifecycleNode::trigger_transition( + const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + return trigger_transition(transition.id(), cb_return_code); +} + +const State & +LifecycleNode::trigger_transition(uint8_t transition_id) +{ + return impl_->trigger_transition(transition_id); +} + +const State & +LifecycleNode::trigger_transition( + uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + return impl_->trigger_transition(transition_id, cb_return_code); +} + +const State & +LifecycleNode::configure() +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); +} + +const State & +LifecycleNode::configure(LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, cb_return_code); +} + +const State & +LifecycleNode::cleanup() +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); +} + +const State & +LifecycleNode::cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, cb_return_code); +} + +const State & +LifecycleNode::activate() +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); +} + +const State & +LifecycleNode::activate(LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, cb_return_code); +} + +const State & +LifecycleNode::deactivate() +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); +} + +const State & +LifecycleNode::deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, cb_return_code); +} + +const State & +LifecycleNode::shutdown() +{ + return impl_->trigger_transition( + rcl_lifecycle_shutdown_label); +} + +const State & +LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + return impl_->trigger_transition( + rcl_lifecycle_shutdown_label, cb_return_code); +} + +void +LifecycleNode::add_publisher_handle( + std::shared_ptr pub) +{ + impl_->add_publisher_handle(pub); +} + +void +LifecycleNode::add_timer_handle(std::shared_ptr timer) +{ + impl_->add_timer_handle(timer); +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp new file mode 100644 index 0000000..5dc81e9 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -0,0 +1,527 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ +#define LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport +#include "lifecycle_msgs/msg/transition_event.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" + +#include "rcl/error_handling.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_lifecycle/transition_map.h" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" + +#include "rcutils/logging_macros.h" + +namespace rclcpp_lifecycle +{ + +class LifecycleNode::LifecycleNodeInterfaceImpl +{ + using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; + using GetStateSrv = lifecycle_msgs::srv::GetState; + using GetAvailableStatesSrv = lifecycle_msgs::srv::GetAvailableStates; + using GetAvailableTransitionsSrv = lifecycle_msgs::srv::GetAvailableTransitions; + using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; + +public: + LifecycleNodeInterfaceImpl( + std::shared_ptr node_base_interface, + std::shared_ptr node_services_interface) + : node_base_interface_(node_base_interface), + node_services_interface_(node_services_interface) + {} + + ~LifecycleNodeInterfaceImpl() + { + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + const rcl_node_options_t * node_options = rcl_node_get_options(node_handle); + auto ret = rcl_lifecycle_state_machine_fini( + &state_machine_, node_handle, &node_options->allocator); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_FATAL_NAMED( + "rclcpp_lifecycle", + "failed to destroy rcl_state_machine"); + } + } + + void + init() + { + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + const rcl_node_options_t * node_options = + rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + // The call to initialize the state machine takes + // currently five different typesupports for all publishers/services + // created within the RCL_LIFECYCLE structure. + // The publisher takes a C-Typesupport since the publishing (i.e. creating + // the message) is done fully in RCL. + // Services are handled in C++, so that it needs a C++ typesupport structure. + rcl_ret_t ret = rcl_lifecycle_state_machine_init( + &state_machine_, + node_handle, + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + true, + &node_options->allocator); + if (ret != RCL_RET_OK) { + throw std::runtime_error( + std::string("Couldn't initialize state machine for node ") + + node_base_interface_->get_name()); + } + + { // change_state + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_change_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_change_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); + } + + { // get_state + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); + } + + { // get_available_states + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_available_states, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_states_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_states, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); + } + + { // get_available_transitions + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_transitions_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); + } + + { // get_transition_graph + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_transition_graph_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_transition_graph, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_transition_graph_), + nullptr); + } + } + + bool + register_callback( + std::uint8_t lifecycle_transition, + std::function & cb) + { + cb_map_[lifecycle_transition] = cb; + return true; + } + + void + on_change_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + + auto transition_id = req->transition.id; + // if there's a label attached to the request, + // we check the transition attached to this label. + // we further can't compare the id of the looked up transition + // because ros2 service call defaults all intergers to zero. + // that means if we call ros2 service call ... {transition: {label: shutdown}} + // the id of the request is 0 (zero) whereas the id from the lookup up transition + // can be different. + // the result of this is that the label takes presedence of the id. + if (req->transition.label.size() != 0) { + auto rcl_transition = rcl_lifecycle_get_transition_by_label( + state_machine_.current_state, req->transition.label.c_str()); + if (rcl_transition == nullptr) { + resp->success = false; + return; + } + transition_id = rcl_transition->id; + } + + node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; + auto ret = change_state(transition_id, cb_return_code); + (void) ret; + // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns + // 1. return is the actual transition + // 2. return is whether an error occurred or not + resp->success = + (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } + + void + on_get_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + resp->current_state.id = static_cast(state_machine_.current_state->id); + resp->current_state.label = state_machine_.current_state->label; + } + + void + on_get_available_states( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available states. State machine is not initialized."); + } + for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) { + lifecycle_msgs::msg::State state; + state.id = static_cast(state_machine_.transition_map.states[i].id); + state.label = static_cast(state_machine_.transition_map.states[i].label); + resp->available_states.push_back(state); + } + } + + void + on_get_available_transitions( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + for (uint8_t i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + auto rcl_transition = state_machine_.current_state->valid_transitions[i]; + lifecycle_msgs::msg::TransitionDescription trans_desc; + trans_desc.transition.id = rcl_transition.id; + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = rcl_transition.start->id; + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = rcl_transition.goal->id; + trans_desc.goal_state.label = rcl_transition.goal->label; + resp->available_transitions.push_back(trans_desc); + } + } + + void + on_get_transition_graph( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) + { + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + auto rcl_transition = state_machine_.transition_map.transitions[i]; + lifecycle_msgs::msg::TransitionDescription trans_desc; + trans_desc.transition.id = rcl_transition.id; + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = rcl_transition.start->id; + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = rcl_transition.goal->id; + trans_desc.goal_state.label = rcl_transition.goal->label; + resp->available_transitions.push_back(trans_desc); + } + } + + const State & + get_current_state() + { + current_state_ = State(state_machine_.current_state); + return current_state_; + } + + std::vector + get_available_states() + { + std::vector states; + for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) { + State state(&state_machine_.transition_map.states[i]); + states.push_back(state); + } + return states; + } + + std::vector + get_available_transitions() + { + std::vector transitions; + + for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + Transition transition( + &state_machine_.transition_map.transitions[i]); + transitions.push_back(transition); + } + return transitions; + } + + rcl_ret_t + change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) + { + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR( + "Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + constexpr bool publish_update = true; + // keep the initial state to pass to a transition callback + State initial_state(state_machine_.current_state); + + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Unable to start transition %u from current state %s: %s", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + + auto get_label_for_return_code = + [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ + auto cb_id = static_cast(cb_return_code); + if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { + return rcl_lifecycle_transition_success_label; + } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { + return rcl_lifecycle_transition_failure_label; + } + return rcl_lifecycle_transition_error_label; + }; + + cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); + auto transition_label = get_label_for_return_code(cb_return_code); + + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, transition_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Failed to finish transition %u. Current state is now: %s (%s)", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + + // error handling ?! + // TODO(karsten1987): iterate over possible ret value + if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { + RCUTILS_LOG_WARN("Error occurred while doing error handling."); + + auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); + auto error_cb_label = get_label_for_return_code(error_cb_code); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + } + // This true holds in both cases where the actual callback + // was successful or not, since at this point we have a valid transistion + // to either a new primary state or error state + return RCL_RET_OK; + } + + LifecycleNodeInterface::CallbackReturn + execute_callback(unsigned int cb_id, const State & previous_state) + { + // in case no callback was attached, we forward directly + auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + auto it = cb_map_.find(cb_id); + if (it != cb_map_.end()) { + auto callback = it->second; + try { + cb_success = callback(State(previous_state)); + } catch (const std::exception & e) { + RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); + RCUTILS_LOG_ERROR("Original error: %s", e.what()); + cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + return cb_success; + } + + const State & trigger_transition(const char * transition_label) + { + LifecycleNodeInterface::CallbackReturn error; + return trigger_transition(transition_label, error); + } + + const State & trigger_transition( + const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code) + { + auto transition = + rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + if (transition) { + change_state(transition->id, cb_return_code); + } + return get_current_state(); + } + + const State & + trigger_transition(uint8_t transition_id) + { + LifecycleNodeInterface::CallbackReturn error; + change_state(transition_id, error); + (void) error; + return get_current_state(); + } + + const State & + trigger_transition(uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) + { + change_state(transition_id, cb_return_code); + return get_current_state(); + } + + void + add_publisher_handle(std::shared_ptr pub) + { + weak_pubs_.push_back(pub); + } + + void + add_timer_handle(std::shared_ptr timer) + { + weak_timers_.push_back(timer); + } + + rcl_lifecycle_state_machine_t state_machine_; + State current_state_; + std::map< + std::uint8_t, + std::function> cb_map_; + + using NodeBasePtr = std::shared_ptr; + using NodeServicesPtr = std::shared_ptr; + using ChangeStateSrvPtr = std::shared_ptr>; + using GetStateSrvPtr = std::shared_ptr>; + using GetAvailableStatesSrvPtr = + std::shared_ptr>; + using GetAvailableTransitionsSrvPtr = + std::shared_ptr>; + using GetTransitionGraphSrvPtr = + std::shared_ptr>; + + NodeBasePtr node_base_interface_; + NodeServicesPtr node_services_interface_; + ChangeStateSrvPtr srv_change_state_; + GetStateSrvPtr srv_get_state_; + GetAvailableStatesSrvPtr srv_get_available_states_; + GetAvailableTransitionsSrvPtr srv_get_available_transitions_; + GetTransitionGraphSrvPtr srv_get_transition_graph_; + + // to controllable things + std::vector> weak_pubs_; + std::vector> weak_timers_; +}; + +} // namespace rclcpp_lifecycle +#endif // LIFECYCLE_NODE_INTERFACE_IMPL_HPP_ diff --git a/rclcpp-eloquent/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp b/rclcpp-eloquent/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp new file mode 100644 index 0000000..1c8a5a6 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -0,0 +1,61 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +namespace rclcpp_lifecycle +{ +namespace node_interfaces +{ + +LifecycleNodeInterface::CallbackReturn +LifecycleNodeInterface::on_configure(const State &) +{ + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn +LifecycleNodeInterface::on_cleanup(const State &) +{ + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn +LifecycleNodeInterface::on_shutdown(const State &) +{ + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn +LifecycleNodeInterface::on_activate(const State &) +{ + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn +LifecycleNodeInterface::on_deactivate(const State &) +{ + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn +LifecycleNodeInterface::on_error(const State &) +{ + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace node_interfaces +} // namespace rclcpp_lifecycle diff --git a/rclcpp-eloquent/rclcpp_lifecycle/src/state.cpp b/rclcpp-eloquent/rclcpp_lifecycle/src/state.cpp new file mode 100644 index 0000000..a84f1d3 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/src/state.cpp @@ -0,0 +1,164 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp_lifecycle/state.hpp" + +#include + +#include "lifecycle_msgs/msg/state.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/allocator.h" + +namespace rclcpp_lifecycle +{ + +State::State(rcutils_allocator_t allocator) +: State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown", allocator) +{} + +State::State( + uint8_t id, + const std::string & label, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_state_handle_(true), + state_handle_(nullptr) +{ + if (label.empty()) { + throw std::runtime_error("Lifecycle State cannot have an empty label."); + } + + state_handle_ = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!state_handle_) { + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + // zero initialize + state_handle_->id = 0; + state_handle_->label = nullptr; + + auto ret = rcl_lifecycle_state_init(state_handle_, id, label.c_str(), &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +State::State( + const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_state_handle_(false), + state_handle_(nullptr) +{ + if (!rcl_lifecycle_state_handle) { + throw std::runtime_error("rcl_lifecycle_state_handle is null"); + } + state_handle_ = const_cast(rcl_lifecycle_state_handle); +} + +State::State(const State & rhs) +: allocator_(rhs.allocator_), + owns_rcl_state_handle_(false), + state_handle_(nullptr) +{ + *this = rhs; +} + +State::~State() +{ + reset(); +} + +State & +State::operator=(const State & rhs) +{ + if (this == &rhs) { + return *this; + } + + // reset all currently used resources + reset(); + + allocator_ = rhs.allocator_; + owns_rcl_state_handle_ = rhs.owns_rcl_state_handle_; + + // we don't own the handle, so we can return straight ahead + if (!owns_rcl_state_handle_) { + state_handle_ = rhs.state_handle_; + return *this; + } + + // we own the handle, so we have to deep-copy the rhs object + state_handle_ = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!state_handle_) { + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + // zero initialize + state_handle_->id = 0; + state_handle_->label = nullptr; + + auto ret = rcl_lifecycle_state_init( + state_handle_, rhs.id(), rhs.label().c_str(), &allocator_); + if (ret != RCL_RET_OK) { + reset(); + throw std::runtime_error("failed to duplicate label for rcl_lifecycle_state_t"); + } + + return *this; +} + +uint8_t +State::id() const +{ + if (!state_handle_) { + throw std::runtime_error("Error in state! Internal state_handle is NULL."); + } + return state_handle_->id; +} + +std::string +State::label() const +{ + if (!state_handle_) { + throw std::runtime_error("Error in state! Internal state_handle is NULL."); + } + return state_handle_->label; +} + +void +State::reset() +{ + if (!owns_rcl_state_handle_) { + state_handle_ = nullptr; + } + + if (!state_handle_) { + return; + } + + auto ret = rcl_lifecycle_state_fini(state_handle_, &allocator_); + allocator_.deallocate(state_handle_, allocator_.state); + state_handle_ = nullptr; + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp-eloquent/rclcpp_lifecycle/src/transition.cpp b/rclcpp-eloquent/rclcpp_lifecycle/src/transition.cpp new file mode 100644 index 0000000..d7bae6c --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/src/transition.cpp @@ -0,0 +1,267 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp_lifecycle/transition.hpp" + +#include + +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/exceptions.hpp" + +#include "rcutils/allocator.h" + +namespace rclcpp_lifecycle +{ + +Transition::Transition( + uint8_t id, + const std::string & label, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_transition_handle_(true), + transition_handle_(nullptr) +{ + transition_handle_ = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_transition_t), allocator_.state)); + if (!transition_handle_) { + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_transition_t"); + } + // zero initialize + transition_handle_->id = 0; + transition_handle_->label = nullptr; + transition_handle_->start = nullptr; + transition_handle_->goal = nullptr; + + auto ret = rcl_lifecycle_transition_init( + transition_handle_, id, label.c_str(), nullptr, nullptr, &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +Transition::Transition( + uint8_t id, const std::string & label, + State && start, State && goal, + rcutils_allocator_t allocator) +: Transition(id, label, allocator) +{ + transition_handle_->start = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!transition_handle_->start) { + reset(); + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + // zero initialize + transition_handle_->start->id = 0; + transition_handle_->start->label = nullptr; + + auto ret = rcl_lifecycle_state_init( + transition_handle_->start, start.id(), start.label().c_str(), &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + transition_handle_->goal = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!transition_handle_->goal) { + reset(); + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + // zero initialize + transition_handle_->goal->id = 0; + transition_handle_->goal->label = nullptr; + + ret = rcl_lifecycle_state_init( + transition_handle_->goal, goal.id(), goal.label().c_str(), &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +Transition::Transition( + const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, + rcutils_allocator_t allocator) +: allocator_(allocator), + owns_rcl_transition_handle_(false), + transition_handle_(nullptr) +{ + if (!rcl_lifecycle_transition_handle) { + throw std::runtime_error("rcl_lifecycle_transition_handle is null"); + } + transition_handle_ = const_cast(rcl_lifecycle_transition_handle); +} + +Transition::Transition(const Transition & rhs) +: allocator_(rhs.allocator_), + owns_rcl_transition_handle_(false), + transition_handle_(nullptr) +{ + *this = rhs; +} + +Transition::~Transition() +{ + reset(); +} + +Transition & +Transition::operator=(const Transition & rhs) +{ + if (this == &rhs) { + return *this; + } + + // reset all currently used resources + reset(); + + allocator_ = rhs.allocator_; + owns_rcl_transition_handle_ = rhs.owns_rcl_transition_handle_; + + // we don't own the handle, so we can return straight ahead + if (!owns_rcl_transition_handle_) { + transition_handle_ = rhs.transition_handle_; + return *this; + } + + // we own the handle, so we have to deep-copy the rhs object + transition_handle_ = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_transition_t), allocator_.state)); + if (!transition_handle_) { + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_transition_t"); + } + // zero initialize + transition_handle_->id = 0; + transition_handle_->label = nullptr; + transition_handle_->start = nullptr; + transition_handle_->goal = nullptr; + + auto ret = rcl_lifecycle_transition_init( + transition_handle_, rhs.id(), rhs.label().c_str(), nullptr, nullptr, &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // only copy start state when available + if (rhs.transition_handle_->start) { + transition_handle_->start = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!transition_handle_->start) { + reset(); + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + // zero initialize + transition_handle_->start->id = 0; + transition_handle_->start->label = nullptr; + + ret = rcl_lifecycle_state_init( + transition_handle_->start, + rhs.start_state().id(), + rhs.start_state().label().c_str(), + &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + + // only copy goal state when available + if (rhs.transition_handle_->goal) { + transition_handle_->goal = static_cast( + allocator_.allocate(sizeof(rcl_lifecycle_state_t), allocator_.state)); + if (!transition_handle_->goal) { + reset(); + throw std::runtime_error("failed to allocate memory for rcl_lifecycle_state_t"); + } + // zero initialize + transition_handle_->goal->id = 0; + transition_handle_->goal->label = nullptr; + + ret = rcl_lifecycle_state_init( + transition_handle_->goal, + rhs.goal_state().id(), + rhs.goal_state().label().c_str(), + &allocator_); + if (ret != RCL_RET_OK) { + reset(); + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + return *this; +} + +uint8_t +Transition::id() const +{ + if (!transition_handle_) { + throw std::runtime_error("internal transition_handle is null"); + } + return transition_handle_->id; +} + +std::string +Transition::label() const +{ + if (!transition_handle_) { + throw std::runtime_error("internal transition_handle is null"); + } + return transition_handle_->label; +} + +State +Transition::start_state() const +{ + if (!transition_handle_) { + throw std::runtime_error("internal transition_handle is null"); + } + // State constructor throws if start pointer is null + return State(transition_handle_->start, allocator_); +} + +State +Transition::goal_state() const +{ + if (!transition_handle_) { + throw std::runtime_error("internal transition_handle is null"); + } + // State constructor throws if goal pointer is null + return State(transition_handle_->goal, allocator_); +} + +void +Transition::reset() +{ + // can't free anything which is not owned + if (!owns_rcl_transition_handle_) { + transition_handle_ = nullptr; + } + + if (!transition_handle_) { + return; + } + + auto ret = rcl_lifecycle_transition_fini(transition_handle_, &allocator_); + allocator_.deallocate(transition_handle_, allocator_.state); + transition_handle_ = nullptr; + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} +} // namespace rclcpp_lifecycle diff --git a/rclcpp-eloquent/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp-eloquent/rclcpp_lifecycle/test/test_callback_exceptions.cpp new file mode 100644 index 0000000..7c4117a --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -0,0 +1,128 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + +class TestCallbackExceptions : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit PositiveCallbackExceptionNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) + { + ++number_of_callbacks; + throw std::runtime_error("custom exception raised in configure callback"); + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_error(const rclcpp_lifecycle::State &) + { + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } +}; + +TEST_F(TestCallbackExceptions, positive_on_error) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + // check if all callbacks were successfully overwritten + EXPECT_EQ(2u, test_node->number_of_callbacks); +} + +TEST_F(TestCallbackExceptions, positive_on_error_with_code) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ret = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + test_node->configure(ret); + EXPECT_EQ(rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR, ret); +} + +class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit NegativeCallbackExceptionNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) + { + ++number_of_callbacks; + throw std::runtime_error("custom exception raised in configure callback"); + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_error(const rclcpp_lifecycle::State &) + { + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } +}; + +TEST_F(TestCallbackExceptions, negative_on_error) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + // check if all callbacks were successfully overwritten + EXPECT_EQ(2u, test_node->number_of_callbacks); +} + +TEST_F(TestCallbackExceptions, negative_on_error_with_code) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ret; + test_node->configure(ret); + EXPECT_EQ(rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR, ret); +} diff --git a/rclcpp-eloquent/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp-eloquent/rclcpp_lifecycle/test/test_lifecycle_node.cpp new file mode 100644 index 0000000..e806637 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -0,0 +1,235 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + +class TestDefaultStateMachine : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit EmptyLifecycleNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} +}; + +struct GoodMood +{ + using CallbackReturnT = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + static constexpr CallbackReturnT cb_ret = static_cast( + lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS); +}; +struct BadMood +{ + using CallbackReturnT = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + static constexpr CallbackReturnT cb_ret = static_cast( + lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE); +}; + +template +class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit MoodyLifecycleNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) + { + EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) + { + EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) + { + EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) + { + EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State &) + { + EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); + ++number_of_callbacks; + return Mood::cb_ret; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_error(const rclcpp_lifecycle::State &); +}; + +template<> +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) +{ + EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); + ADD_FAILURE(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +template<> +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) +{ + EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +TEST_F(TestDefaultStateMachine, empty_initializer) { + auto test_node = std::make_shared("testnode"); + EXPECT_STREQ("testnode", test_node->get_name()); + EXPECT_STREQ("/", test_node->get_namespace()); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); +} + +TEST_F(TestDefaultStateMachine, trigger_transition) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + ASSERT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + ASSERT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + ASSERT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); +} + +TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) { + auto test_node = std::make_shared("testnode"); + + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto ret = reset_key; + + test_node->configure(ret); + + EXPECT_EQ(success, ret); + ret = reset_key; + + test_node->activate(ret); + EXPECT_EQ(success, ret); + ret = reset_key; + + test_node->deactivate(ret); + EXPECT_EQ(success, ret); + ret = reset_key; + + test_node->cleanup(ret); + EXPECT_EQ(success, ret); + ret = reset_key; + + test_node->shutdown(ret); + EXPECT_EQ(success, ret); +} + +TEST_F(TestDefaultStateMachine, good_mood) { + auto test_node = std::make_shared>("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(5u, test_node->number_of_callbacks); +} + +TEST_F(TestDefaultStateMachine, bad_mood) { + auto test_node = std::make_shared>("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(1u, test_node->number_of_callbacks); +} + +TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { + auto test_node = std::make_shared>("testnode"); + + auto cb = [](const std::shared_ptr msg) {(void) msg;}; + auto lifecycle_sub = + test_node->create_subscription("~/empty", 10, cb); + + SUCCEED(); +} diff --git a/rclcpp-eloquent/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp-eloquent/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp new file mode 100644 index 0000000..3c146e4 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -0,0 +1,180 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + +class TestRegisterCustomCallbacks : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit CustomLifecycleNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + {} + + size_t number_of_callbacks = 0; + +protected: + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State &) + { + ADD_FAILURE(); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + // Custom callbacks + +public: + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_custom_configure(const rclcpp_lifecycle::State & previous_state) + { + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_custom_activate(const rclcpp_lifecycle::State & previous_state) + { + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_custom_deactivate(const rclcpp_lifecycle::State & previous_state) + { + EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_custom_cleanup(const rclcpp_lifecycle::State & previous_state) + { + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_custom_shutdown(const rclcpp_lifecycle::State &) + { + EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); + ++number_of_callbacks; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } +}; + +TEST_F(TestRegisterCustomCallbacks, custom_callbacks) { + auto test_node = std::make_shared("testnode"); + + test_node->register_on_configure( + std::bind( + &CustomLifecycleNode::on_custom_configure, + test_node.get(), std::placeholders::_1)); + test_node->register_on_cleanup( + std::bind( + &CustomLifecycleNode::on_custom_cleanup, + test_node.get(), std::placeholders::_1)); + test_node->register_on_shutdown( + std::bind( + &CustomLifecycleNode::on_custom_shutdown, + test_node.get(), std::placeholders::_1)); + test_node->register_on_activate( + std::bind( + &CustomLifecycleNode::on_custom_activate, + test_node.get(), std::placeholders::_1)); + test_node->register_on_deactivate( + std::bind( + &CustomLifecycleNode::on_custom_deactivate, + test_node.get(), std::placeholders::_1)); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(5u, test_node->number_of_callbacks); +} diff --git a/rclcpp-eloquent/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp-eloquent/rclcpp_lifecycle/test/test_state_machine_info.cpp new file mode 100644 index 0000000..dfa7bdf --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -0,0 +1,75 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class TestStateMachineInfo : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST_F(TestStateMachineInfo, available_states) { + auto test_node = std::make_shared("testnode"); + std::vector available_states = + test_node->get_available_states(); + EXPECT_EQ(11u, available_states.size()); + + // Primary States + EXPECT_EQ(0, available_states[0].id()); // unknown + EXPECT_EQ(1, available_states[1].id()); // unconfigured + EXPECT_EQ(2, available_states[2].id()); // inactive + EXPECT_EQ(3, available_states[3].id()); // active + EXPECT_EQ(4, available_states[4].id()); // finalized + + // Transition States + EXPECT_EQ(10, available_states[5].id()); // configuring + EXPECT_EQ(11, available_states[6].id()); // cleaningup + EXPECT_EQ(12, available_states[7].id()); // shuttingdown + EXPECT_EQ(13, available_states[8].id()); // activating + EXPECT_EQ(14, available_states[9].id()); // deactivating + EXPECT_EQ(15, available_states[10].id()); // errorprocessing +} + +TEST_F(TestStateMachineInfo, available_transitions) { + auto test_node = std::make_shared("testnode"); + std::vector available_transitions = + test_node->get_available_transitions(); + EXPECT_EQ(25u, available_transitions.size()); + for (rclcpp_lifecycle::Transition & transition : available_transitions) { + EXPECT_FALSE(transition.label().empty()); + + EXPECT_TRUE( + transition.start_state().id() <= 4 || + (transition.start_state().id() >= 10 && + (transition.start_state().id() <= 15))); + EXPECT_FALSE(transition.start_state().label().empty()); + + EXPECT_TRUE( + transition.goal_state().id() <= 4 || + (transition.goal_state().id() >= 10 && + (transition.goal_state().id() <= 15))); + EXPECT_FALSE(transition.goal_state().label().empty()); + } +} diff --git a/rclcpp-eloquent/rclcpp_lifecycle/test/test_state_wrapper.cpp b/rclcpp-eloquent/rclcpp_lifecycle/test/test_state_wrapper.cpp new file mode 100644 index 0000000..5d20759 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/test/test_state_wrapper.cpp @@ -0,0 +1,168 @@ +// Copyright 2017 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 +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class TestStateWrapper : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + } +}; + +TEST_F(TestStateWrapper, wrapper) { + { + rclcpp_lifecycle::State state(1, "my_state"); + EXPECT_EQ(1, state.id()); + EXPECT_FALSE(state.label().empty()); + EXPECT_STREQ("my_state", state.label().c_str()); + } + + { + std::string state_name("my_state"); + rclcpp_lifecycle::State state(1, state_name); + state_name = "not_my_state"; + EXPECT_STREQ("my_state", state.label().c_str()); + } + + { + rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, 0}; + rclcpp_lifecycle::State c_state(lc_state.id, lc_state.label); + EXPECT_EQ(2, c_state.id()); + EXPECT_FALSE(c_state.label().empty()); + EXPECT_STREQ("my_c_state", c_state.label().c_str()); + } + + { + rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, 0}; + rclcpp_lifecycle::State c_state(&lc_state); + EXPECT_EQ(2, c_state.id()); + EXPECT_FALSE(c_state.label().empty()); + EXPECT_STREQ("my_c_state", c_state.label().c_str()); + } + + { + rcl_lifecycle_state_t * lc_state = + new rcl_lifecycle_state_t {"my_c_state", 3, NULL, 0}; + rclcpp_lifecycle::State c_state(lc_state->id, lc_state->label); + EXPECT_EQ(3, c_state.id()); + EXPECT_FALSE(c_state.label().empty()); + EXPECT_STREQ("my_c_state", c_state.label().c_str()); + delete lc_state; + } + + // introduces flakiness + // unsupported behavior! + // fails when compiled with memory sanitizer + // { + // rcl_lifecycle_state_t * lc_state + // = new rcl_lifecycle_state_t {"my_c_state", 3, NULL, NULL, 0}; + // rclcpp_lifecycle::State c_state(lc_state); + // delete lc_state; + // lc_state = NULL; + // EXPECT_EQ(3, c_state.id()); + // EXPECT_STREQ("my_c_state", c_state.label().c_str()); + // } +} + +TEST_F(TestStateWrapper, copy_constructor) { + auto a = std::make_shared(1, "my_c_state"); + rclcpp_lifecycle::State b(*a); + + a.reset(); + + EXPECT_EQ(1, b.id()); + EXPECT_STREQ("my_c_state", b.label().c_str()); +} + +TEST_F(TestStateWrapper, assignment_operator) { + auto a = std::make_shared(1, "one"); + auto b = std::make_shared(2, "two"); + *b = *a; + + a.reset(); + + EXPECT_EQ(1, b->id()); + EXPECT_STREQ("one", b->label().c_str()); +} + +TEST_F(TestStateWrapper, assignment_operator2) { + // Non-owning State + rcl_lifecycle_state_t * lc_state1 = + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; + auto non_owning_state1 = std::make_shared(lc_state1); + + // Non-owning State + rcl_lifecycle_state_t * lc_state2 = + new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, 0}; + auto non_owning_state2 = std::make_shared(lc_state2); + + *non_owning_state2 = *non_owning_state1; + + EXPECT_EQ(1, non_owning_state2->id()); + EXPECT_STREQ("my_c_state1", non_owning_state2->label().c_str()); + + non_owning_state1.reset(); + non_owning_state2.reset(); + + delete lc_state1; + delete lc_state2; +} + +TEST_F(TestStateWrapper, assignment_operator3) { + // Non-owning State + rcl_lifecycle_state_t * lc_state1 = + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; + auto non_owning_state1 = std::make_shared(lc_state1); + + // owning State + auto owning_state2 = std::make_shared(2, "my_c_state2"); + + *owning_state2 = *non_owning_state1; + + EXPECT_EQ(1, owning_state2->id()); + EXPECT_STREQ("my_c_state1", owning_state2->label().c_str()); + + non_owning_state1.reset(); + owning_state2.reset(); + + delete lc_state1; +} + +TEST_F(TestStateWrapper, assignment_operator4) { + // Non-owning State + rcl_lifecycle_state_t * lc_state1 = + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; + auto non_owning_state1 = std::make_shared(lc_state1); + + // owning State + auto owning_state2 = std::make_shared(2, "my_c_state2"); + + *non_owning_state1 = *owning_state2; + + EXPECT_EQ(2, non_owning_state1->id()); + EXPECT_STREQ("my_c_state2", non_owning_state1->label().c_str()); + + non_owning_state1.reset(); + owning_state2.reset(); + + delete lc_state1; +} diff --git a/rclcpp-eloquent/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp-eloquent/rclcpp_lifecycle/test/test_transition_wrapper.cpp new file mode 100644 index 0000000..b920c74 --- /dev/null +++ b/rclcpp-eloquent/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -0,0 +1,86 @@ +// Copyright 2017 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 +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +class TestTransitionWrapper : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + } +}; + +TEST_F(TestTransitionWrapper, empty_transition) { + auto a = std::make_shared(1, "my_transition"); + EXPECT_NO_THROW(a.reset()); +} + +TEST_F(TestTransitionWrapper, wrapper) { + { + rclcpp_lifecycle::Transition t(12, "no_states_set"); + EXPECT_EQ(12, t.id()); + EXPECT_STREQ("no_states_set", t.label().c_str()); + } + + { + std::string transition_name = "no_states_set"; + rclcpp_lifecycle::Transition t(12, transition_name); + transition_name = "not_no_states_set"; + EXPECT_EQ(12, t.id()); + EXPECT_STREQ("no_states_set", t.label().c_str()); + } + + { + rclcpp_lifecycle::State start_state(1, "start_state"); + rclcpp_lifecycle::State goal_state(2, "goal_state"); + + rclcpp_lifecycle::Transition t( + 12, + "from_start_to_goal", + std::move(start_state), + std::move(goal_state)); + + EXPECT_EQ(12, t.id()); + EXPECT_FALSE(t.label().empty()); + EXPECT_STREQ("from_start_to_goal", t.label().c_str()); + } +} + +TEST_F(TestTransitionWrapper, copy_constructor) { + auto a = std::make_shared(1, "my_transition"); + rclcpp_lifecycle::Transition b(*a); + + a.reset(); + + EXPECT_EQ(1, b.id()); + EXPECT_STREQ("my_transition", b.label().c_str()); +} + +TEST_F(TestTransitionWrapper, assignment_operator) { + auto a = std::make_shared(1, "one"); + auto b = std::make_shared(2, "two"); + *b = *a; + + a.reset(); + + EXPECT_EQ(1, b->id()); + EXPECT_STREQ("one", b->label().c_str()); +}