Compare commits
27 Commits
28.0.0
...
runtime_in
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
32859cdf3c | ||
|
|
30210a2c02 | ||
|
|
d871af007a | ||
|
|
29577dab24 | ||
|
|
000f788b65 | ||
|
|
ca17caf6c4 | ||
|
|
e7e9a298c0 | ||
|
|
8571b28664 | ||
|
|
7e0d063154 | ||
|
|
113dc01dc7 | ||
|
|
aac28e7b9f | ||
|
|
24e533eba7 | ||
|
|
ca1fe2b41b | ||
|
|
57900a9f44 | ||
|
|
363861f70c | ||
|
|
9555859691 | ||
|
|
78a568d14a | ||
|
|
9ff28439d1 | ||
|
|
bfcc29e64b | ||
|
|
927b9de2a5 | ||
|
|
5af57f8010 | ||
|
|
0ce81c4fed | ||
|
|
a2484be7c3 | ||
|
|
833efe0e2d | ||
|
|
554ad022a6 | ||
|
|
a3e5d0a91a | ||
|
|
b8fd304546 |
@@ -2,210 +2,6 @@
|
||||
Changelog for package rclcpp
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
28.0.0 (2024-03-28)
|
||||
-------------------
|
||||
* fix spin_some_max_duration unit-test for events-executor (`#2465 <https://github.com/ros2/rclcpp/issues/2465>`_)
|
||||
* refactor and improve the parameterized spin_some tests for executors (`#2460 <https://github.com/ros2/rclcpp/issues/2460>`_)
|
||||
* refactor and improve the spin_some parameterized tests for executors
|
||||
* disable spin_some_max_duration for the StaticSingleThreadedExecutor and EventsExecutor
|
||||
* fixup and clarify the docstring for Executor::spin_some()
|
||||
* style
|
||||
* review comments
|
||||
---------
|
||||
* enable simulation clock for timer canceling test. (`#2458 <https://github.com/ros2/rclcpp/issues/2458>`_)
|
||||
* enable simulation clock for timer canceling test.
|
||||
* move MainExecutorTypes to test_executors_timer_cancel_behavior.cpp.
|
||||
---------
|
||||
* Revert "relax the test simulation rate for timer canceling tests. (`#2453 <https://github.com/ros2/rclcpp/issues/2453>`_)" (`#2456 <https://github.com/ros2/rclcpp/issues/2456>`_)
|
||||
This reverts commit 1c350d0d7fb9c7158e0a39057112486ddbd38e9a.
|
||||
* relax the test simulation rate for timer canceling tests. (`#2453 <https://github.com/ros2/rclcpp/issues/2453>`_)
|
||||
* Fix TypeAdapted publishing with large messages. (`#2443 <https://github.com/ros2/rclcpp/issues/2443>`_)
|
||||
Mostly by ensuring we aren't attempting to store
|
||||
large messages on the stack. Also add in tests.
|
||||
I verified that before these changes, the tests failed,
|
||||
while after them they succeed.
|
||||
* Implement generic client (`#2358 <https://github.com/ros2/rclcpp/issues/2358>`_)
|
||||
* Implement generic client
|
||||
* Fix the incorrect parameter declaration
|
||||
* Deleted copy constructor and assignment for FutureAndRequestId
|
||||
* Update codes after rebase
|
||||
* Address review comments
|
||||
* Address review comments from iuhilnehc-ynos
|
||||
* Correct an error in a description
|
||||
* Fix window build errors
|
||||
* Address review comments from William
|
||||
* Add doc strings to create_generic_client
|
||||
---------
|
||||
* Rule of five: implement move operators (`#2425 <https://github.com/ros2/rclcpp/issues/2425>`_)
|
||||
* Various cleanups to deal with uncrustify 0.78. (`#2439 <https://github.com/ros2/rclcpp/issues/2439>`_)
|
||||
These should also work with uncrustify 0.72.
|
||||
* Remove the set_deprecated signatures in any_subscription_callback. (`#2431 <https://github.com/ros2/rclcpp/issues/2431>`_)
|
||||
These have been deprecated since April 2021, so it is safe
|
||||
to remove them now.
|
||||
* fix doxygen syntax for NodeInterfaces (`#2428 <https://github.com/ros2/rclcpp/issues/2428>`_)
|
||||
* Set hints to find the python version we actually want. (`#2426 <https://github.com/ros2/rclcpp/issues/2426>`_)
|
||||
The comment in the commit explains the reasoning behind it.
|
||||
* Update quality declaration documents (`#2427 <https://github.com/ros2/rclcpp/issues/2427>`_)
|
||||
* feat: add/minus for msg::Time and rclcpp::Duration (`#2419 <https://github.com/ros2/rclcpp/issues/2419>`_)
|
||||
* feat: add/minus for msg::Time and rclcpp::Duration
|
||||
* Contributors: Alberto Soragna, Barry Xu, Chris Lalancette, Christophe Bedard, HuaTsai, Jonas Otto, Tim Clephas, Tomoya Fujita, William Woodall
|
||||
|
||||
27.0.0 (2024-02-07)
|
||||
-------------------
|
||||
* Split test_executors up into smaller chunks. (`#2421 <https://github.com/ros2/rclcpp/issues/2421>`_)
|
||||
* [events executor] - Fix Behavior with Timer Cancel (`#2375 <https://github.com/ros2/rclcpp/issues/2375>`_)
|
||||
* Removed deprecated header (`#2413 <https://github.com/ros2/rclcpp/issues/2413>`_)
|
||||
* Make sure to mark RingBuffer methods as 'override'. (`#2410 <https://github.com/ros2/rclcpp/issues/2410>`_)
|
||||
* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Matt Condino
|
||||
|
||||
26.0.0 (2024-01-24)
|
||||
-------------------
|
||||
* Increase the cppcheck timeout to 600 seconds. (`#2409 <https://github.com/ros2/rclcpp/issues/2409>`_)
|
||||
* Add transient local durability support to publisher and subscriptions when using intra-process communication (`#2303 <https://github.com/ros2/rclcpp/issues/2303>`_)
|
||||
* Stop storing the context in the guard condition. (`#2400 <https://github.com/ros2/rclcpp/issues/2400>`_)
|
||||
* Contributors: Chris Lalancette, Jeffery Hsu
|
||||
|
||||
25.0.0 (2023-12-26)
|
||||
-------------------
|
||||
* Updated GenericSubscription to AnySubscriptionCallback (`#1928 <https://github.com/ros2/rclcpp/issues/1928>`_)
|
||||
* make type support helper supported for service (`#2209 <https://github.com/ros2/rclcpp/issues/2209>`_)
|
||||
* Adding QoS to subscription options (`#2323 <https://github.com/ros2/rclcpp/issues/2323>`_)
|
||||
* Switch to target_link_libraries. (`#2374 <https://github.com/ros2/rclcpp/issues/2374>`_)
|
||||
* aligh with rcl that a rosout publisher of a node might not exist (`#2357 <https://github.com/ros2/rclcpp/issues/2357>`_)
|
||||
* Fix data race in EventHandlerBase (`#2349 <https://github.com/ros2/rclcpp/issues/2349>`_)
|
||||
* Support users holding onto shared pointers in the message memory pool (`#2336 <https://github.com/ros2/rclcpp/issues/2336>`_)
|
||||
* Contributors: Chen Lihui, Chris Lalancette, DensoADAS, Lucas Wendland, mauropasse
|
||||
|
||||
24.0.0 (2023-11-06)
|
||||
-------------------
|
||||
* fix (signal_handler.hpp): spelling (`#2356 <https://github.com/ros2/rclcpp/issues/2356>`_)
|
||||
* Updates to not use std::move in some places. (`#2353 <https://github.com/ros2/rclcpp/issues/2353>`_)
|
||||
* rclcpp::Time::max() clock type support. (`#2352 <https://github.com/ros2/rclcpp/issues/2352>`_)
|
||||
* Serialized Messages with Topic Statistics (`#2274 <https://github.com/ros2/rclcpp/issues/2274>`_)
|
||||
* Add a custom deleter when constructing rcl_service_t (`#2351 <https://github.com/ros2/rclcpp/issues/2351>`_)
|
||||
* Disable the loaned messages inside the executor. (`#2335 <https://github.com/ros2/rclcpp/issues/2335>`_)
|
||||
* Use message_info in SubscriptionTopicStatistics instead of typed message (`#2337 <https://github.com/ros2/rclcpp/issues/2337>`_)
|
||||
* Add missing 'enable_rosout' comments (`#2345 <https://github.com/ros2/rclcpp/issues/2345>`_)
|
||||
* Adjust rclcpp usage of type description service (`#2344 <https://github.com/ros2/rclcpp/issues/2344>`_)
|
||||
* address rate related flaky tests. (`#2329 <https://github.com/ros2/rclcpp/issues/2329>`_)
|
||||
* Fixes pointed out by the clang analyzer. (`#2339 <https://github.com/ros2/rclcpp/issues/2339>`_)
|
||||
* Remove useless ROSRate class (`#2326 <https://github.com/ros2/rclcpp/issues/2326>`_)
|
||||
* Contributors: Alexey Merzlyakov, Chris Lalancette, Jiaqi Li, Lucas Wendland, Michael Carroll, Michael Orlov, Tomoya Fujita, Zard-C
|
||||
|
||||
23.2.0 (2023-10-09)
|
||||
-------------------
|
||||
* add clients & services count (`#2072 <https://github.com/ros2/rclcpp/issues/2072>`_)
|
||||
* remove invalid sized allocation test for SerializedMessage. (`#2330 <https://github.com/ros2/rclcpp/issues/2330>`_)
|
||||
* Adding API to copy all parameters from one node to another (`#2304 <https://github.com/ros2/rclcpp/issues/2304>`_)
|
||||
* Contributors: Minju, Lee, Steve Macenski, Tomoya Fujita
|
||||
|
||||
23.1.0 (2023-10-04)
|
||||
-------------------
|
||||
* Add locking to protect the TimeSource::NodeState::node_base\_ (`#2320 <https://github.com/ros2/rclcpp/issues/2320>`_)
|
||||
* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 <https://github.com/ros2/rclcpp/issues/2316>`_)
|
||||
* Removing Old Connext Tests (`#2313 <https://github.com/ros2/rclcpp/issues/2313>`_)
|
||||
* Documentation for list_parameters (`#2315 <https://github.com/ros2/rclcpp/issues/2315>`_)
|
||||
* Decouple rosout publisher init from node init. (`#2174 <https://github.com/ros2/rclcpp/issues/2174>`_)
|
||||
* fix the depth to relative in list_parameters (`#2300 <https://github.com/ros2/rclcpp/issues/2300>`_)
|
||||
* Contributors: Chris Lalancette, Lucas Wendland, Minju, Lee, Tomoya Fujita, Tully Foote
|
||||
|
||||
23.0.0 (2023-09-08)
|
||||
-------------------
|
||||
* Fix the return type of Rate::period. (`#2301 <https://github.com/ros2/rclcpp/issues/2301>`_)
|
||||
* Update API docs links in package READMEs (`#2302 <https://github.com/ros2/rclcpp/issues/2302>`_)
|
||||
* Cleanup flaky timers_manager tests. (`#2299 <https://github.com/ros2/rclcpp/issues/2299>`_)
|
||||
* Contributors: Chris Lalancette, Christophe Bedard
|
||||
|
||||
22.2.0 (2023-09-07)
|
||||
-------------------
|
||||
* Topic correct typeadapter deduction (`#2294 <https://github.com/ros2/rclcpp/issues/2294>`_)
|
||||
* Fix C++20 allocator construct deprecation (`#2292 <https://github.com/ros2/rclcpp/issues/2292>`_)
|
||||
* Make Rate to select the clock to work with (`#2123 <https://github.com/ros2/rclcpp/issues/2123>`_)
|
||||
* Correct the position of a comment. (`#2290 <https://github.com/ros2/rclcpp/issues/2290>`_)
|
||||
* Remove unnecessary lambda captures in the tests. (`#2289 <https://github.com/ros2/rclcpp/issues/2289>`_)
|
||||
* Add rcl_logging_interface as an explicit dependency. (`#2284 <https://github.com/ros2/rclcpp/issues/2284>`_)
|
||||
* Revamp list_parameters to be more efficient and easier to read. (`#2282 <https://github.com/ros2/rclcpp/issues/2282>`_)
|
||||
* Contributors: AiVerisimilitude, Alexey Merzlyakov, Chen Lihui, Chris Lalancette, Jiaqi Li
|
||||
|
||||
22.1.0 (2023-08-21)
|
||||
-------------------
|
||||
* Do not crash Executor when send_response fails due to client failure. (`#2276 <https://github.com/ros2/rclcpp/issues/2276>`_)
|
||||
* Adding Custom Unknown Type Error (`#2272 <https://github.com/ros2/rclcpp/issues/2272>`_)
|
||||
* Add a pimpl inside rclcpp::Node for future distro backports (`#2228 <https://github.com/ros2/rclcpp/issues/2228>`_)
|
||||
* Remove an unused variable from the events executor tests. (`#2270 <https://github.com/ros2/rclcpp/issues/2270>`_)
|
||||
* Add spin_all shortcut (`#2246 <https://github.com/ros2/rclcpp/issues/2246>`_)
|
||||
* Adding Missing Group Exceptions (`#2256 <https://github.com/ros2/rclcpp/issues/2256>`_)
|
||||
* Change associated clocks storage to unordered_set (`#2257 <https://github.com/ros2/rclcpp/issues/2257>`_)
|
||||
* associated clocks should be protected by mutex. (`#2255 <https://github.com/ros2/rclcpp/issues/2255>`_)
|
||||
* Instrument loaned message publication code path (`#2240 <https://github.com/ros2/rclcpp/issues/2240>`_)
|
||||
* Contributors: Chris Lalancette, Christophe Bedard, Emerson Knapp, Luca Della Vedova, Lucas Wendland, Tomoya Fujita, Tony Najjar
|
||||
|
||||
22.0.0 (2023-07-11)
|
||||
-------------------
|
||||
* Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 <https://github.com/ros2/rclcpp/issues/2237>`_)
|
||||
* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 <https://github.com/ros2/rclcpp/issues/2224>`_)
|
||||
* Move always_false_v to detail namespace (`#2232 <https://github.com/ros2/rclcpp/issues/2232>`_)
|
||||
* Revamp the test_subscription.cpp tests. (`#2227 <https://github.com/ros2/rclcpp/issues/2227>`_)
|
||||
* warning: comparison of integer expressions of different signedness (`#2219 <https://github.com/ros2/rclcpp/issues/2219>`_)
|
||||
* Modifies timers API to select autostart state (`#2005 <https://github.com/ros2/rclcpp/issues/2005>`_)
|
||||
* Enable callback group tests for connextdds (`#2182 <https://github.com/ros2/rclcpp/issues/2182>`_)
|
||||
* Contributors: Chris Lalancette, Christopher Wecht, Eloy Briceno, Emerson Knapp, Nathan Wiebe Neufeldt, Tomoya Fujita
|
||||
|
||||
21.3.0 (2023-06-12)
|
||||
-------------------
|
||||
* Fix up misspellings of "receive". (`#2208 <https://github.com/ros2/rclcpp/issues/2208>`_)
|
||||
* Remove flaky stressAddRemoveNode test (`#2206 <https://github.com/ros2/rclcpp/issues/2206>`_)
|
||||
* Use TRACETOOLS\_ prefix for tracepoint-related macros (`#2162 <https://github.com/ros2/rclcpp/issues/2162>`_)
|
||||
* Contributors: Chris Lalancette, Christophe Bedard, Michael Carroll
|
||||
|
||||
21.2.0 (2023-06-07)
|
||||
-------------------
|
||||
* remove nolint since ament_cpplint updated for the c++17 header (`#2198 <https://github.com/ros2/rclcpp/issues/2198>`_)
|
||||
* Feature/available capacity of ipm (`#2173 <https://github.com/ros2/rclcpp/issues/2173>`_)
|
||||
* add mutex to protect events_executor current entity collection (`#2187 <https://github.com/ros2/rclcpp/issues/2187>`_)
|
||||
* Declare rclcpp callbacks before the rcl entities (`#2024 <https://github.com/ros2/rclcpp/issues/2024>`_)
|
||||
* Contributors: Alberto Soragna, Chen Lihui, DensoADAS, mauropasse
|
||||
|
||||
21.1.1 (2023-05-11)
|
||||
-------------------
|
||||
* Fix race condition in events-executor (`#2177 <https://github.com/ros2/rclcpp/issues/2177>`_)
|
||||
* Add missing stdexcept include (`#2186 <https://github.com/ros2/rclcpp/issues/2186>`_)
|
||||
* Fix a format-security warning when building with clang (`#2171 <https://github.com/ros2/rclcpp/issues/2171>`_)
|
||||
* Fix delivered message kind (`#2175 <https://github.com/ros2/rclcpp/issues/2175>`_)
|
||||
* Contributors: Alberto Soragna, Chris Lalancette, methylDragon, Øystein Sture
|
||||
|
||||
21.1.0 (2023-04-27)
|
||||
-------------------
|
||||
|
||||
21.0.0 (2023-04-18)
|
||||
-------------------
|
||||
* Add support for logging service. (`#2122 <https://github.com/ros2/rclcpp/issues/2122>`_)
|
||||
* Picking ABI-incompatible executor changes (`#2170 <https://github.com/ros2/rclcpp/issues/2170>`_)
|
||||
* add events-executor and timers-manager in rclcpp (`#2155 <https://github.com/ros2/rclcpp/issues/2155>`_)
|
||||
* Create common structures for executors to use (`#2143 <https://github.com/ros2/rclcpp/issues/2143>`_)
|
||||
* Implement deliver message kind (`#2168 <https://github.com/ros2/rclcpp/issues/2168>`_)
|
||||
* Contributors: Alberto Soragna, Lei Liu, Michael Carroll, methylDragon
|
||||
|
||||
20.0.0 (2023-04-13)
|
||||
-------------------
|
||||
* applied tracepoints for ring_buffer (`#2091 <https://github.com/ros2/rclcpp/issues/2091>`_)
|
||||
* Dynamic Subscription (REP-2011 Subset): Stubs for rclcpp (`#2165 <https://github.com/ros2/rclcpp/issues/2165>`_)
|
||||
* Add type_hash to cpp TopicEndpointInfo (`#2137 <https://github.com/ros2/rclcpp/issues/2137>`_)
|
||||
* Trigger the intraprocess guard condition with data (`#2164 <https://github.com/ros2/rclcpp/issues/2164>`_)
|
||||
* Minor grammar fix (`#2149 <https://github.com/ros2/rclcpp/issues/2149>`_)
|
||||
* Fix unnecessary allocations in executor.cpp (`#2135 <https://github.com/ros2/rclcpp/issues/2135>`_)
|
||||
* add Logger::get_effective_level(). (`#2141 <https://github.com/ros2/rclcpp/issues/2141>`_)
|
||||
* Remove deprecated header (`#2139 <https://github.com/ros2/rclcpp/issues/2139>`_)
|
||||
* Implement matched event (`#2105 <https://github.com/ros2/rclcpp/issues/2105>`_)
|
||||
* use allocator via init_options argument. (`#2129 <https://github.com/ros2/rclcpp/issues/2129>`_)
|
||||
* Fixes to silence some clang warnings. (`#2127 <https://github.com/ros2/rclcpp/issues/2127>`_)
|
||||
* Documentation improvements on the executor (`#2125 <https://github.com/ros2/rclcpp/issues/2125>`_)
|
||||
* Avoid losing waitable handles while using MultiThreadedExecutor (`#2109 <https://github.com/ros2/rclcpp/issues/2109>`_)
|
||||
* Hook up the incompatible type event inside of rclcpp (`#2069 <https://github.com/ros2/rclcpp/issues/2069>`_)
|
||||
* Update all rclcpp packages to C++17. (`#2121 <https://github.com/ros2/rclcpp/issues/2121>`_)
|
||||
* Fix clang warning: bugprone-use-after-move (`#2116 <https://github.com/ros2/rclcpp/issues/2116>`_)
|
||||
* Contributors: Barry Xu, Chris Lalancette, Christopher Wecht, Emerson Knapp, Michael Carroll, Tomoya Fujita, Yadu, mauropasse, methylDragon, ymski
|
||||
|
||||
19.3.0 (2023-03-01)
|
||||
-------------------
|
||||
* Fix memory leak in tracetools::get_symbol() (`#2104 <https://github.com/ros2/rclcpp/issues/2104>`_)
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.20)
|
||||
cmake_minimum_required(VERSION 3.12)
|
||||
|
||||
project(rclcpp)
|
||||
|
||||
@@ -10,14 +10,11 @@ find_package(builtin_interfaces REQUIRED)
|
||||
find_package(libstatistics_collector REQUIRED)
|
||||
find_package(rcl REQUIRED)
|
||||
find_package(rcl_interfaces REQUIRED)
|
||||
find_package(rcl_logging_interface REQUIRED)
|
||||
find_package(rcl_yaml_param_parser REQUIRED)
|
||||
find_package(rcpputils REQUIRED)
|
||||
find_package(rcutils REQUIRED)
|
||||
find_package(rmw REQUIRED)
|
||||
find_package(rosgraph_msgs REQUIRED)
|
||||
find_package(rosidl_dynamic_typesupport REQUIRED)
|
||||
find_package(rosidl_runtime_c REQUIRED)
|
||||
find_package(rosidl_runtime_cpp REQUIRED)
|
||||
find_package(rosidl_typesupport_c REQUIRED)
|
||||
find_package(rosidl_typesupport_cpp REQUIRED)
|
||||
@@ -45,9 +42,7 @@ set(${PROJECT_NAME}_SRCS
|
||||
src/rclcpp/clock.cpp
|
||||
src/rclcpp/context.cpp
|
||||
src/rclcpp/contexts/default_context.cpp
|
||||
src/rclcpp/create_generic_client.cpp
|
||||
src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp
|
||||
src/rclcpp/detail/resolve_intra_process_buffer_type.cpp
|
||||
src/rclcpp/detail/resolve_parameter_overrides.cpp
|
||||
src/rclcpp/detail/rmw_implementation_specific_payload.cpp
|
||||
src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
|
||||
@@ -64,18 +59,12 @@ set(${PROJECT_NAME}_SRCS
|
||||
src/rclcpp/executable_list.cpp
|
||||
src/rclcpp/executor.cpp
|
||||
src/rclcpp/executors.cpp
|
||||
src/rclcpp/executors/executor_entities_collection.cpp
|
||||
src/rclcpp/executors/executor_entities_collector.cpp
|
||||
src/rclcpp/executors/executor_notify_waitable.cpp
|
||||
src/rclcpp/executors/multi_threaded_executor.cpp
|
||||
src/rclcpp/executors/single_threaded_executor.cpp
|
||||
src/rclcpp/executors/static_executor_entities_collector.cpp
|
||||
src/rclcpp/executors/static_single_threaded_executor.cpp
|
||||
src/rclcpp/expand_topic_or_service_name.cpp
|
||||
src/rclcpp/experimental/executors/events_executor/events_executor.cpp
|
||||
src/rclcpp/experimental/timers_manager.cpp
|
||||
src/rclcpp/future_return_code.cpp
|
||||
src/rclcpp/generic_client.cpp
|
||||
src/rclcpp/generic_publisher.cpp
|
||||
src/rclcpp/generic_subscription.cpp
|
||||
src/rclcpp/graph_listener.cpp
|
||||
@@ -98,7 +87,6 @@ set(${PROJECT_NAME}_SRCS
|
||||
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_type_descriptions.cpp
|
||||
src/rclcpp/node_interfaces/node_waitables.cpp
|
||||
src/rclcpp/node_options.cpp
|
||||
src/rclcpp/parameter.cpp
|
||||
@@ -112,7 +100,7 @@ set(${PROJECT_NAME}_SRCS
|
||||
src/rclcpp/qos.cpp
|
||||
src/rclcpp/event_handler.cpp
|
||||
src/rclcpp/qos_overriding_options.cpp
|
||||
src/rclcpp/rate.cpp
|
||||
src/rclcpp/dynamic_subscription.cpp
|
||||
src/rclcpp/serialization.cpp
|
||||
src/rclcpp/serialized_message.cpp
|
||||
src/rclcpp/service.cpp
|
||||
@@ -129,21 +117,6 @@ set(${PROJECT_NAME}_SRCS
|
||||
src/rclcpp/waitable.cpp
|
||||
)
|
||||
|
||||
# By default, without the settings below, find_package(Python3) will attempt
|
||||
# to find the newest python version it can, and additionally will find the
|
||||
# most specific version. For instance, on a system that has
|
||||
# /usr/bin/python3.10, /usr/bin/python3.11, and /usr/bin/python3, it will find
|
||||
# /usr/bin/python3.11, even if /usr/bin/python3 points to /usr/bin/python3.10.
|
||||
# The behavior we want is to prefer the "system" installed version unless the
|
||||
# user specifically tells us othewise through the Python3_EXECUTABLE hint.
|
||||
# Setting CMP0094 to NEW means that the search will stop after the first
|
||||
# python version is found. Setting Python3_FIND_UNVERSIONED_NAMES means that
|
||||
# the search will prefer /usr/bin/python3 over /usr/bin/python3.11. And that
|
||||
# latter functionality is only available in CMake 3.20 or later, so we need
|
||||
# at least that version.
|
||||
cmake_policy(SET CMP0094 NEW)
|
||||
set(Python3_FIND_UNVERSIONED_NAMES FIRST)
|
||||
|
||||
find_package(Python3 REQUIRED COMPONENTS Interpreter)
|
||||
|
||||
# "watch" template for changes
|
||||
@@ -222,28 +195,22 @@ target_include_directories(${PROJECT_NAME} PUBLIC
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
|
||||
"$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
|
||||
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
|
||||
target_link_libraries(${PROJECT_NAME} PUBLIC
|
||||
${builtin_interfaces_TARGETS}
|
||||
libstatistics_collector::libstatistics_collector
|
||||
rcl::rcl
|
||||
${rcl_interfaces_TARGETS}
|
||||
rcl_yaml_param_parser::rcl_yaml_param_parser
|
||||
rcpputils::rcpputils
|
||||
rcutils::rcutils
|
||||
rmw::rmw
|
||||
${rosgraph_msgs_TARGETS}
|
||||
rosidl_dynamic_typesupport::rosidl_dynamic_typesupport
|
||||
rosidl_runtime_c::rosidl_runtime_c
|
||||
rosidl_runtime_cpp::rosidl_runtime_cpp
|
||||
rosidl_typesupport_cpp::rosidl_typesupport_cpp
|
||||
${statistics_msgs_TARGETS}
|
||||
tracetools::tracetools
|
||||
${CMAKE_THREAD_LIBS_INIT}
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME} PRIVATE
|
||||
ament_index_cpp::ament_index_cpp
|
||||
rcl_logging_interface::rcl_logging_interface
|
||||
target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
|
||||
# specific order: dependents before dependencies
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
"ament_index_cpp"
|
||||
"libstatistics_collector"
|
||||
"rcl"
|
||||
"rcl_interfaces"
|
||||
"rcl_yaml_param_parser"
|
||||
"rcpputils"
|
||||
"rcutils"
|
||||
"builtin_interfaces"
|
||||
"rosgraph_msgs"
|
||||
"rosidl_typesupport_cpp"
|
||||
"rosidl_runtime_cpp"
|
||||
"statistics_msgs"
|
||||
"tracetools"
|
||||
)
|
||||
|
||||
# Causes the visibility macros to use dllexport rather than dllimport,
|
||||
@@ -265,23 +232,20 @@ ament_export_libraries(${PROJECT_NAME})
|
||||
# Export modern CMake targets
|
||||
ament_export_targets(${PROJECT_NAME})
|
||||
|
||||
ament_export_dependencies(
|
||||
builtin_interfaces
|
||||
libstatistics_collector
|
||||
rcl
|
||||
rcl_interfaces
|
||||
rcl_yaml_param_parser
|
||||
rcpputils
|
||||
rcutils
|
||||
rmw
|
||||
rosgraph_msgs
|
||||
rosidl_dynamic_typesupport
|
||||
rosidl_runtime_c
|
||||
rosidl_runtime_cpp
|
||||
rosidl_typesupport_cpp
|
||||
statistics_msgs
|
||||
tracetools
|
||||
)
|
||||
# specific order: dependents before dependencies
|
||||
ament_export_dependencies(ament_index_cpp)
|
||||
ament_export_dependencies(libstatistics_collector)
|
||||
ament_export_dependencies(rcl)
|
||||
ament_export_dependencies(rcpputils)
|
||||
ament_export_dependencies(rcutils)
|
||||
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_runtime_cpp)
|
||||
ament_export_dependencies(rcl_yaml_param_parser)
|
||||
ament_export_dependencies(statistics_msgs)
|
||||
ament_export_dependencies(tracetools)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
@@ -299,7 +263,7 @@ install(
|
||||
|
||||
if(TEST cppcheck)
|
||||
# must set the property after ament_package()
|
||||
set_tests_properties(cppcheck PROPERTIES TIMEOUT 600)
|
||||
set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
|
||||
endif()
|
||||
|
||||
if(TEST cpplint)
|
||||
|
||||
@@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp` package, bas
|
||||
|
||||
The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
|
||||
|
||||
Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html).
|
||||
Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide.
|
||||
|
||||
## Version Policy [1]
|
||||
|
||||
@@ -94,7 +94,7 @@ There is an automated test which runs a linter that ensures each file has at lea
|
||||
|
||||
### Feature Testing [4.i]
|
||||
|
||||
Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/rolling/test) directory.
|
||||
Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/master/test) directory.
|
||||
New features are required to have tests before being added.
|
||||
|
||||
Currently nightly test results can be seen here:
|
||||
@@ -129,7 +129,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly
|
||||
|
||||
`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change.
|
||||
|
||||
The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp/test/benchmark).
|
||||
The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp/test/benchmark).
|
||||
|
||||
System level performance benchmarks that cover features of `rclcpp` can be found at:
|
||||
* [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/)
|
||||
@@ -139,7 +139,7 @@ Changes that introduce regressions in performance must be adequately justified i
|
||||
|
||||
### Linters and Static Analysis [4.v]
|
||||
|
||||
`rclcpp` uses and passes all the ROS 2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
|
||||
`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms.
|
||||
|
||||
Currently nightly test results can be seen here:
|
||||
* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/)
|
||||
@@ -163,49 +163,49 @@ It also has several test dependencies, which do not affect the resulting quality
|
||||
|
||||
The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/rolling/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
|
||||
|
||||
#### `rcl`
|
||||
|
||||
`rcl` a library to support implementation of language specific ROS 2 Client Libraries.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
|
||||
|
||||
#### `rcl_yaml_param_parser`
|
||||
|
||||
The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/rolling/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
|
||||
|
||||
#### `rcpputils`
|
||||
|
||||
The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).
|
||||
|
||||
#### `rcutils`
|
||||
|
||||
The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/rolling/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).
|
||||
|
||||
#### `rmw`
|
||||
|
||||
`rmw` is the ROS 2 middleware library.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/rmw/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md).
|
||||
|
||||
#### `statistics_msgs`
|
||||
|
||||
The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/statistics_msgs/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
|
||||
|
||||
#### `tracetools`
|
||||
|
||||
The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis.
|
||||
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/ros2_tracing/blob/rolling/tracetools/QUALITY_DECLARATION.md).
|
||||
It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
|
||||
|
||||
### Direct Runtime non-ROS Dependency [5.iii]
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
The ROS client library in C++.
|
||||
|
||||
The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
|
||||
Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components and features.
|
||||
|
||||
## Quality Declaration
|
||||
|
||||
|
||||
@@ -156,7 +156,7 @@ public:
|
||||
const std::shared_ptr<rmw_request_id_t> & request_header,
|
||||
std::shared_ptr<typename ServiceT::Request> request)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
|
||||
TRACEPOINT(callback_start, static_cast<const void *>(this), false);
|
||||
if (std::holds_alternative<std::monostate>(callback_)) {
|
||||
// TODO(ivanpauno): Remove the set method, and force the users of this class
|
||||
// to pass a callback at construnciton.
|
||||
@@ -182,7 +182,7 @@ public:
|
||||
const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
|
||||
cb(request_header, std::move(request), response);
|
||||
}
|
||||
TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
return response;
|
||||
}
|
||||
|
||||
@@ -191,9 +191,9 @@ public:
|
||||
#ifndef TRACETOOLS_DISABLED
|
||||
std::visit(
|
||||
[this](auto && arg) {
|
||||
if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
char * symbol = tracetools::get_symbol(arg);
|
||||
TRACETOOLS_DO_TRACEPOINT(
|
||||
DO_TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
static_cast<const void *>(this),
|
||||
symbol);
|
||||
|
||||
@@ -20,7 +20,7 @@
|
||||
#include <stdexcept>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
#include <variant>
|
||||
#include <variant> // NOLINT[build/include_order]
|
||||
|
||||
#include "rosidl_runtime_cpp/traits.hpp"
|
||||
#include "tracetools/tracetools.h"
|
||||
@@ -30,19 +30,19 @@
|
||||
#include "rclcpp/detail/subscription_callback_type_helper.hpp"
|
||||
#include "rclcpp/function_traits.hpp"
|
||||
#include "rclcpp/message_info.hpp"
|
||||
#include "rclcpp/serialization.hpp"
|
||||
#include "rclcpp/serialized_message.hpp"
|
||||
#include "rclcpp/type_adapter.hpp"
|
||||
|
||||
|
||||
template<class>
|
||||
inline constexpr bool always_false_v = false;
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template<class>
|
||||
inline constexpr bool always_false_v = false;
|
||||
|
||||
template<typename MessageT, typename AllocatorT>
|
||||
struct MessageDeleterHelper
|
||||
{
|
||||
@@ -158,14 +158,13 @@ struct AnySubscriptionCallbackPossibleTypes
|
||||
template<
|
||||
typename MessageT,
|
||||
typename AllocatorT,
|
||||
bool is_adapted_type = rclcpp::TypeAdapter<MessageT>::is_specialized::value,
|
||||
bool is_serialized_type = serialization_traits::is_serialized_message_class<MessageT>::value
|
||||
bool is_adapted_type = rclcpp::TypeAdapter<MessageT>::is_specialized::value
|
||||
>
|
||||
struct AnySubscriptionCallbackHelper;
|
||||
|
||||
/// Specialization for when MessageT is not a TypeAdapter.
|
||||
template<typename MessageT, typename AllocatorT>
|
||||
struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false, false>
|
||||
struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false>
|
||||
{
|
||||
using CallbackTypes = AnySubscriptionCallbackPossibleTypes<MessageT, AllocatorT>;
|
||||
|
||||
@@ -195,7 +194,7 @@ struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false, false>
|
||||
|
||||
/// Specialization for when MessageT is a TypeAdapter.
|
||||
template<typename MessageT, typename AllocatorT>
|
||||
struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, true, false>
|
||||
struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, true>
|
||||
{
|
||||
using CallbackTypes = AnySubscriptionCallbackPossibleTypes<MessageT, AllocatorT>;
|
||||
|
||||
@@ -233,26 +232,6 @@ struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, true, false>
|
||||
>;
|
||||
};
|
||||
|
||||
/// Specialization for when MessageT is a SerializedMessage to exclude duplicated declarations.
|
||||
template<typename MessageT, typename AllocatorT>
|
||||
struct AnySubscriptionCallbackHelper<MessageT, AllocatorT, false, true>
|
||||
{
|
||||
using CallbackTypes = AnySubscriptionCallbackPossibleTypes<MessageT, AllocatorT>;
|
||||
|
||||
using variant_type = std::variant<
|
||||
typename CallbackTypes::ConstRefSerializedMessageCallback,
|
||||
typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback,
|
||||
typename CallbackTypes::UniquePtrSerializedMessageCallback,
|
||||
typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback,
|
||||
typename CallbackTypes::SharedConstPtrSerializedMessageCallback,
|
||||
typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback,
|
||||
typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback,
|
||||
typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback,
|
||||
typename CallbackTypes::SharedPtrSerializedMessageCallback,
|
||||
typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback
|
||||
>;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
|
||||
template<
|
||||
@@ -393,12 +372,58 @@ public:
|
||||
// converted to one another, e.g. shared_ptr and unique_ptr.
|
||||
using scbth = detail::SubscriptionCallbackTypeHelper<MessageT, CallbackT>;
|
||||
|
||||
callback_variant_ = static_cast<typename scbth::callback_type>(callback);
|
||||
// Determine if the given CallbackT is a deprecated signature or not.
|
||||
constexpr auto is_deprecated =
|
||||
rclcpp::function_traits::same_arguments<
|
||||
typename scbth::callback_type,
|
||||
std::function<void(std::shared_ptr<MessageT>)>
|
||||
>::value
|
||||
||
|
||||
rclcpp::function_traits::same_arguments<
|
||||
typename scbth::callback_type,
|
||||
std::function<void(std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>
|
||||
>::value;
|
||||
|
||||
// Use the discovered type to force the type of callback when assigning
|
||||
// into the variant.
|
||||
if constexpr (is_deprecated) {
|
||||
// If deprecated, call sub-routine that is deprecated.
|
||||
set_deprecated(static_cast<typename scbth::callback_type>(callback));
|
||||
} else {
|
||||
// Otherwise just assign it.
|
||||
callback_variant_ = static_cast<typename scbth::callback_type>(callback);
|
||||
}
|
||||
|
||||
// Return copy of self for easier testing, normally will be compiled out.
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Function for shared_ptr to non-const MessageT, which is deprecated.
|
||||
template<typename SetT>
|
||||
#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
|
||||
// suppress deprecation warnings in `test_any_subscription_callback.cpp`
|
||||
[[deprecated("use 'void(std::shared_ptr<const MessageT>)' instead")]]
|
||||
#endif
|
||||
void
|
||||
set_deprecated(std::function<void(std::shared_ptr<SetT>)> callback)
|
||||
{
|
||||
callback_variant_ = callback;
|
||||
}
|
||||
|
||||
/// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated.
|
||||
template<typename SetT>
|
||||
#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS)
|
||||
// suppress deprecation warnings in `test_any_subscription_callback.cpp`
|
||||
[[deprecated(
|
||||
"use 'void(std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)' instead"
|
||||
)]]
|
||||
#endif
|
||||
void
|
||||
set_deprecated(std::function<void(std::shared_ptr<SetT>, const rclcpp::MessageInfo &)> callback)
|
||||
{
|
||||
callback_variant_ = callback;
|
||||
}
|
||||
|
||||
std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
|
||||
create_ros_unique_ptr_from_ros_shared_ptr_message(
|
||||
const std::shared_ptr<const ROSMessageType> & message)
|
||||
@@ -462,14 +487,12 @@ public:
|
||||
}
|
||||
|
||||
// Dispatch when input is a ros message and the output could be anything.
|
||||
template<typename TMsg = ROSMessageType>
|
||||
typename std::enable_if<!serialization_traits::is_serialized_message_class<TMsg>::value,
|
||||
void>::type
|
||||
void
|
||||
dispatch(
|
||||
std::shared_ptr<ROSMessageType> message,
|
||||
const rclcpp::MessageInfo & message_info)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
|
||||
TRACEPOINT(callback_start, static_cast<const void *>(this), false);
|
||||
// Check if the variant is "unset", throw if it is.
|
||||
if (callback_variant_.index() == 0) {
|
||||
if (std::get<0>(callback_variant_) == nullptr) {
|
||||
@@ -557,19 +580,19 @@ public:
|
||||
}
|
||||
// condition to catch unhandled callback types
|
||||
else { // NOLINT[readability/braces]
|
||||
static_assert(detail::always_false_v<T>, "unhandled callback type");
|
||||
static_assert(always_false_v<T>, "unhandled callback type");
|
||||
}
|
||||
}, callback_variant_);
|
||||
TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
}
|
||||
|
||||
// Dispatch when input is a serialized message and the output could be anything.
|
||||
void
|
||||
dispatch(
|
||||
std::shared_ptr<const rclcpp::SerializedMessage> serialized_message,
|
||||
std::shared_ptr<rclcpp::SerializedMessage> serialized_message,
|
||||
const rclcpp::MessageInfo & message_info)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), false);
|
||||
TRACEPOINT(callback_start, static_cast<const void *>(this), false);
|
||||
// Check if the variant is "unset", throw if it is.
|
||||
if (callback_variant_.index() == 0) {
|
||||
if (std::get<0>(callback_variant_) == nullptr) {
|
||||
@@ -637,10 +660,10 @@ public:
|
||||
}
|
||||
// condition to catch unhandled callback types
|
||||
else { // NOLINT[readability/braces]
|
||||
static_assert(detail::always_false_v<T>, "unhandled callback type");
|
||||
static_assert(always_false_v<T>, "unhandled callback type");
|
||||
}
|
||||
}, callback_variant_);
|
||||
TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
}
|
||||
|
||||
void
|
||||
@@ -648,7 +671,7 @@ public:
|
||||
std::shared_ptr<const SubscribedType> message,
|
||||
const rclcpp::MessageInfo & message_info)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), true);
|
||||
TRACEPOINT(callback_start, static_cast<const void *>(this), true);
|
||||
// Check if the variant is "unset", throw if it is.
|
||||
if (callback_variant_.index() == 0) {
|
||||
if (std::get<0>(callback_variant_) == nullptr) {
|
||||
@@ -767,10 +790,10 @@ public:
|
||||
}
|
||||
// condition to catch unhandled callback types
|
||||
else { // NOLINT[readability/braces]
|
||||
static_assert(detail::always_false_v<T>, "unhandled callback type");
|
||||
static_assert(always_false_v<T>, "unhandled callback type");
|
||||
}
|
||||
}, callback_variant_);
|
||||
TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
}
|
||||
|
||||
void
|
||||
@@ -778,7 +801,7 @@ public:
|
||||
std::unique_ptr<SubscribedType, SubscribedTypeDeleter> message,
|
||||
const rclcpp::MessageInfo & message_info)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(callback_start, static_cast<const void *>(this), true);
|
||||
TRACEPOINT(callback_start, static_cast<const void *>(this), true);
|
||||
// Check if the variant is "unset", throw if it is.
|
||||
if (callback_variant_.index() == 0) {
|
||||
if (std::get<0>(callback_variant_) == nullptr) {
|
||||
@@ -901,10 +924,10 @@ public:
|
||||
}
|
||||
// condition to catch unhandled callback types
|
||||
else { // NOLINT[readability/braces]
|
||||
static_assert(detail::always_false_v<T>, "unhandled callback type");
|
||||
static_assert(always_false_v<T>, "unhandled callback type");
|
||||
}
|
||||
}, callback_variant_);
|
||||
TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
TRACEPOINT(callback_end, static_cast<const void *>(this));
|
||||
}
|
||||
|
||||
constexpr
|
||||
@@ -942,9 +965,9 @@ public:
|
||||
#ifndef TRACETOOLS_DISABLED
|
||||
std::visit(
|
||||
[this](auto && callback) {
|
||||
if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
char * symbol = tracetools::get_symbol(callback);
|
||||
TRACETOOLS_DO_TRACEPOINT(
|
||||
DO_TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
static_cast<const void *>(this),
|
||||
symbol);
|
||||
|
||||
@@ -93,53 +93,11 @@ public:
|
||||
* determines whether a callback group is automatically added to an executor
|
||||
* with the node with which it is associated.
|
||||
*/
|
||||
[[deprecated("Use CallbackGroup constructor with context function argument")]]
|
||||
RCLCPP_PUBLIC
|
||||
explicit CallbackGroup(
|
||||
CallbackGroupType group_type,
|
||||
bool automatically_add_to_executor_with_node = true);
|
||||
|
||||
/// Constructor for CallbackGroup.
|
||||
/**
|
||||
* Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
|
||||
* and when creating one the type must be specified.
|
||||
*
|
||||
* Callbacks in Reentrant Callback Groups must be able to:
|
||||
* - run at the same time as themselves (reentrant)
|
||||
* - run at the same time as other callbacks in their group
|
||||
* - run at the same time as other callbacks in other groups
|
||||
*
|
||||
* Callbacks in Mutually Exclusive Callback Groups:
|
||||
* - will not be run multiple times simultaneously (non-reentrant)
|
||||
* - will not be run at the same time as other callbacks in their group
|
||||
* - but must run at the same time as callbacks in other groups
|
||||
*
|
||||
* Additionally, callback groups have a property which determines whether or
|
||||
* not they are added to an executor with their associated node automatically.
|
||||
* When creating a callback group the automatically_add_to_executor_with_node
|
||||
* argument determines this behavior, and if true it will cause the newly
|
||||
* created callback group to be added to an executor with the node when the
|
||||
* Executor::add_node method is used.
|
||||
* If false, this callback group will not be added automatically and would
|
||||
* have to be added to an executor manually using the
|
||||
* Executor::add_callback_group method.
|
||||
*
|
||||
* Whether the node was added to the executor before creating the callback
|
||||
* group, or after, is irrelevant; the callback group will be automatically
|
||||
* added to the executor in either case.
|
||||
*
|
||||
* \param[in] group_type The type of the callback group.
|
||||
* \param[in] context A weak pointer to the context associated with this callback group.
|
||||
* \param[in] automatically_add_to_executor_with_node A boolean that
|
||||
* determines whether a callback group is automatically added to an executor
|
||||
* with the node with which it is associated.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit CallbackGroup(
|
||||
CallbackGroupType group_type,
|
||||
rclcpp::Context::WeakPtr context,
|
||||
bool automatically_add_to_executor_with_node = true);
|
||||
|
||||
/// Default destructor.
|
||||
RCLCPP_PUBLIC
|
||||
~CallbackGroup();
|
||||
@@ -179,13 +137,6 @@ public:
|
||||
return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
|
||||
}
|
||||
|
||||
/// Get the total number of entities in this callback group.
|
||||
/**
|
||||
* \return the number of entities in the callback group.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t size() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::atomic_bool &
|
||||
can_be_taken_from();
|
||||
@@ -227,13 +178,10 @@ public:
|
||||
bool
|
||||
automatically_add_to_executor_with_node() const;
|
||||
|
||||
/// Retrieve the guard condition used to signal changes to this callback group.
|
||||
/**
|
||||
* \return guard condition if it is valid, otherwise nullptr.
|
||||
*/
|
||||
/// Defer creating the notify guard condition and return it.
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
get_notify_guard_condition();
|
||||
get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
|
||||
|
||||
/// Trigger the notify guard condition.
|
||||
RCLCPP_PUBLIC
|
||||
@@ -286,8 +234,6 @@ protected:
|
||||
std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
|
||||
std::recursive_mutex notify_guard_condition_mutex_;
|
||||
|
||||
rclcpp::Context::WeakPtr context_;
|
||||
|
||||
private:
|
||||
template<typename TypeT, typename Function>
|
||||
typename TypeT::SharedPtr _find_ptrs_if_impl(
|
||||
|
||||
@@ -20,13 +20,13 @@
|
||||
#include <future>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <optional> // NOLINT, cpplint doesn't think this is a cpp std header
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <variant>
|
||||
#include <variant> // NOLINT
|
||||
#include <vector>
|
||||
|
||||
#include "rcl/client.h"
|
||||
@@ -115,29 +115,6 @@ struct FutureAndRequestId
|
||||
/// Destructor.
|
||||
~FutureAndRequestId() = default;
|
||||
};
|
||||
|
||||
template<typename PendingRequestsT, typename AllocatorT = std::allocator<int64_t>>
|
||||
size_t
|
||||
prune_requests_older_than_impl(
|
||||
PendingRequestsT & pending_requests,
|
||||
std::mutex & pending_requests_mutex,
|
||||
std::chrono::time_point<std::chrono::system_clock> time_point,
|
||||
std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
|
||||
{
|
||||
std::lock_guard guard(pending_requests_mutex);
|
||||
auto old_size = pending_requests.size();
|
||||
for (auto it = pending_requests.begin(), last = pending_requests.end(); it != last; ) {
|
||||
if (it->second.first < time_point) {
|
||||
if (pruned_requests) {
|
||||
pruned_requests->push_back(it->first);
|
||||
}
|
||||
it = pending_requests.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
return old_size - pending_requests.size();
|
||||
}
|
||||
} // namespace detail
|
||||
|
||||
namespace node_interfaces
|
||||
@@ -386,16 +363,12 @@ protected:
|
||||
std::shared_ptr<rclcpp::Context> context_;
|
||||
rclcpp::Logger node_logger_;
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
// It is important to declare on_new_response_callback_ before
|
||||
// client_handle_, so on destruction the client is
|
||||
// destroyed first. Otherwise, the rmw client callback
|
||||
// would point briefly to a destroyed function.
|
||||
std::function<void(size_t)> on_new_response_callback_{nullptr};
|
||||
// Declare client_handle_ after callback
|
||||
std::shared_ptr<rcl_client_t> client_handle_;
|
||||
|
||||
std::atomic<bool> in_use_by_wait_set_{false};
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
std::function<void(size_t)> on_new_response_callback_{nullptr};
|
||||
};
|
||||
|
||||
template<typename ServiceT>
|
||||
@@ -794,11 +767,19 @@ public:
|
||||
std::chrono::time_point<std::chrono::system_clock> time_point,
|
||||
std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
|
||||
{
|
||||
return detail::prune_requests_older_than_impl(
|
||||
pending_requests_,
|
||||
pending_requests_mutex_,
|
||||
time_point,
|
||||
pruned_requests);
|
||||
std::lock_guard guard(pending_requests_mutex_);
|
||||
auto old_size = pending_requests_.size();
|
||||
for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
|
||||
if (it->second.first < time_point) {
|
||||
if (pruned_requests) {
|
||||
pruned_requests->push_back(it->first);
|
||||
}
|
||||
it = pending_requests_.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
return old_size - pending_requests_.size();
|
||||
}
|
||||
|
||||
/// Configure client introspection.
|
||||
|
||||
@@ -26,7 +26,6 @@
|
||||
#include <unordered_set>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "rcl/context.h"
|
||||
#include "rcl/guard_condition.h"
|
||||
|
||||
@@ -1,82 +0,0 @@
|
||||
// Copyright 2023 Open Navigation LLC
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__COPY_ALL_PARAMETER_VALUES_HPP_
|
||||
#define RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rcl_interfaces/srv/list_parameters.hpp"
|
||||
#include "rcl_interfaces/msg/parameter_descriptor.hpp"
|
||||
#include "rcl_interfaces/msg/set_parameters_result.hpp"
|
||||
|
||||
#include "rclcpp/parameter.hpp"
|
||||
#include "rclcpp/logger.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
/**
|
||||
* Copy all parameters from one source node to another destination node.
|
||||
* May throw exceptions if parameters from source are uninitialized or undeclared.
|
||||
* \param source Node to copy parameters from
|
||||
* \param destination Node to copy parameters to
|
||||
* \param override_existing_params Default false. Whether to override existing destination params
|
||||
* if both the source and destination contain the same parameter.
|
||||
*/
|
||||
template<typename NodeT1, typename NodeT2>
|
||||
void
|
||||
copy_all_parameter_values(
|
||||
const NodeT1 & source, const NodeT2 & destination, const bool override_existing_params = false)
|
||||
{
|
||||
using Parameters = std::vector<rclcpp::Parameter>;
|
||||
using Descriptions = std::vector<rcl_interfaces::msg::ParameterDescriptor>;
|
||||
auto source_params = source->get_node_parameters_interface();
|
||||
auto dest_params = destination->get_node_parameters_interface();
|
||||
rclcpp::Logger logger = destination->get_node_logging_interface()->get_logger();
|
||||
|
||||
std::vector<std::string> param_names = source_params->list_parameters({}, 0).names;
|
||||
Parameters params = source_params->get_parameters(param_names);
|
||||
Descriptions descriptions = source_params->describe_parameters(param_names);
|
||||
|
||||
for (unsigned int idx = 0; idx != params.size(); idx++) {
|
||||
if (!dest_params->has_parameter(params[idx].get_name())) {
|
||||
dest_params->declare_parameter(
|
||||
params[idx].get_name(), params[idx].get_parameter_value(), descriptions[idx]);
|
||||
} else if (override_existing_params) {
|
||||
try {
|
||||
rcl_interfaces::msg::SetParametersResult result =
|
||||
dest_params->set_parameters_atomically({params[idx]});
|
||||
if (!result.successful) {
|
||||
// Parameter update rejected or read-only
|
||||
RCLCPP_WARN(
|
||||
logger,
|
||||
"Unable to set parameter (%s): %s!",
|
||||
params[idx].get_name().c_str(), result.reason.c_str());
|
||||
}
|
||||
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
|
||||
RCLCPP_WARN(
|
||||
logger,
|
||||
"Unable to set parameter (%s): incompatable parameter type (%s)!",
|
||||
params[idx].get_name().c_str(), e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_
|
||||
@@ -1,90 +0,0 @@
|
||||
// Copyright 2023 Sony Group Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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_GENERIC_CLIENT_HPP_
|
||||
#define RCLCPP__CREATE_GENERIC_CLIENT_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/generic_client.hpp"
|
||||
#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/get_node_graph_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/get_node_services_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_services_interface.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
/// Create a generic service client with a name of given type.
|
||||
/**
|
||||
* \param[in] node_base NodeBaseInterface implementation of the node on which
|
||||
* to create the client.
|
||||
* \param[in] node_graph NodeGraphInterface implementation of the node on which
|
||||
* to create the client.
|
||||
* \param[in] node_services NodeServicesInterface implementation of the node on
|
||||
* which to create the client.
|
||||
* \param[in] service_name The name on which the service is accessible.
|
||||
* \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes"
|
||||
* \param[in] qos Quality of service profile for client.
|
||||
* \param[in] group Callback group to handle the reply to service calls.
|
||||
* \return Shared pointer to the created client.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GenericClient::SharedPtr
|
||||
create_generic_client(
|
||||
std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
|
||||
std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
|
||||
std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
|
||||
const std::string & service_name,
|
||||
const std::string & service_type,
|
||||
const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr);
|
||||
|
||||
/// Create a generic service client with a name of given type.
|
||||
/**
|
||||
* The NodeT type needs to have NodeBaseInterface implementation, NodeGraphInterface implementation
|
||||
* and NodeServicesInterface implementation of the node which to create the client.
|
||||
*
|
||||
* \param[in] node The node on which to create the client.
|
||||
* \param[in] service_name The name on which the service is accessible.
|
||||
* \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes"
|
||||
* \param[in] qos Quality of service profile for client.
|
||||
* \param[in] group Callback group to handle the reply to service calls.
|
||||
* \return Shared pointer to the created client.
|
||||
*/
|
||||
template<typename NodeT>
|
||||
rclcpp::GenericClient::SharedPtr
|
||||
create_generic_client(
|
||||
NodeT node,
|
||||
const std::string & service_name,
|
||||
const std::string & service_type,
|
||||
const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr)
|
||||
{
|
||||
return create_generic_client(
|
||||
rclcpp::node_interfaces::get_node_base_interface(node),
|
||||
rclcpp::node_interfaces::get_node_graph_interface(node),
|
||||
rclcpp::node_interfaces::get_node_services_interface(node),
|
||||
service_name,
|
||||
service_type,
|
||||
qos,
|
||||
group
|
||||
);
|
||||
}
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__CREATE_GENERIC_CLIENT_HPP_
|
||||
@@ -45,15 +45,13 @@ namespace rclcpp
|
||||
* Not all publisher options are currently respected, the only relevant options for this
|
||||
* publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`.
|
||||
*/
|
||||
template<
|
||||
typename CallbackT,
|
||||
typename AllocatorT = std::allocator<void>>
|
||||
template<typename AllocatorT = std::allocator<void>>
|
||||
std::shared_ptr<GenericSubscription> create_generic_subscription(
|
||||
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
|
||||
const std::string & topic_name,
|
||||
const std::string & topic_type,
|
||||
const rclcpp::QoS & qos,
|
||||
CallbackT && callback,
|
||||
std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
|
||||
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
|
||||
rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
|
||||
)
|
||||
@@ -62,20 +60,13 @@ std::shared_ptr<GenericSubscription> create_generic_subscription(
|
||||
auto ts_lib = rclcpp::get_typesupport_library(
|
||||
topic_type, "rosidl_typesupport_cpp");
|
||||
|
||||
auto allocator = options.get_allocator();
|
||||
|
||||
using rclcpp::AnySubscriptionCallback;
|
||||
AnySubscriptionCallback<rclcpp::SerializedMessage, AllocatorT>
|
||||
any_subscription_callback(*allocator);
|
||||
any_subscription_callback.set(std::forward<CallbackT>(callback));
|
||||
|
||||
auto subscription = std::make_shared<GenericSubscription>(
|
||||
topics_interface->get_node_base_interface(),
|
||||
std::move(ts_lib),
|
||||
topic_name,
|
||||
topic_type,
|
||||
qos,
|
||||
any_subscription_callback,
|
||||
callback,
|
||||
options);
|
||||
|
||||
topics_interface->add_subscription(subscription, options.callback_group);
|
||||
|
||||
@@ -50,8 +50,8 @@ template<
|
||||
typename SubscriptionT,
|
||||
typename MessageMemoryStrategyT,
|
||||
typename NodeParametersT,
|
||||
typename NodeTopicsT
|
||||
>
|
||||
typename NodeTopicsT,
|
||||
typename ROSMessageType = typename SubscriptionT::ROSMessageType>
|
||||
typename std::shared_ptr<SubscriptionT>
|
||||
create_subscription(
|
||||
NodeParametersT & node_parameters,
|
||||
@@ -70,7 +70,7 @@ create_subscription(
|
||||
using rclcpp::node_interfaces::get_node_topics_interface;
|
||||
auto node_topics_interface = get_node_topics_interface(node_topics);
|
||||
|
||||
std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
|
||||
std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>
|
||||
subscription_topic_stats = nullptr;
|
||||
|
||||
if (rclcpp::detail::resolve_enable_topic_statistics(
|
||||
@@ -80,7 +80,8 @@ create_subscription(
|
||||
if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) {
|
||||
throw std::invalid_argument(
|
||||
"topic_stats_options.publish_period must be greater than 0, specified value of " +
|
||||
std::to_string(options.topic_stats_options.publish_period.count()) + " ms");
|
||||
std::to_string(options.topic_stats_options.publish_period.count()) +
|
||||
" ms");
|
||||
}
|
||||
|
||||
std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>>
|
||||
@@ -88,14 +89,14 @@ create_subscription(
|
||||
node_parameters,
|
||||
node_topics_interface,
|
||||
options.topic_stats_options.publish_topic,
|
||||
options.topic_stats_options.qos);
|
||||
qos);
|
||||
|
||||
subscription_topic_stats =
|
||||
std::make_shared<rclcpp::topic_statistics::SubscriptionTopicStatistics>(
|
||||
node_topics_interface->get_node_base_interface()->get_name(), publisher);
|
||||
subscription_topic_stats = std::make_shared<
|
||||
rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>
|
||||
>(node_topics_interface->get_node_base_interface()->get_name(), publisher);
|
||||
|
||||
std::weak_ptr<
|
||||
rclcpp::topic_statistics::SubscriptionTopicStatistics
|
||||
rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>
|
||||
> weak_subscription_topic_stats(subscription_topic_stats);
|
||||
auto sub_call_back = [weak_subscription_topic_stats]() {
|
||||
auto subscription_topic_stats = weak_subscription_topic_stats.lock();
|
||||
|
||||
@@ -90,8 +90,7 @@ create_timer(
|
||||
rclcpp::Clock::SharedPtr clock,
|
||||
rclcpp::Duration period,
|
||||
CallbackT && callback,
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr,
|
||||
bool autostart = true)
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr)
|
||||
{
|
||||
return create_timer(
|
||||
clock,
|
||||
@@ -99,8 +98,7 @@ create_timer(
|
||||
std::forward<CallbackT>(callback),
|
||||
group,
|
||||
node_base.get(),
|
||||
node_timers.get(),
|
||||
autostart);
|
||||
node_timers.get());
|
||||
}
|
||||
|
||||
/// Create a timer with a given clock
|
||||
@@ -111,8 +109,7 @@ create_timer(
|
||||
rclcpp::Clock::SharedPtr clock,
|
||||
rclcpp::Duration period,
|
||||
CallbackT && callback,
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr,
|
||||
bool autostart = true)
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr)
|
||||
{
|
||||
return create_timer(
|
||||
clock,
|
||||
@@ -120,8 +117,7 @@ create_timer(
|
||||
std::forward<CallbackT>(callback),
|
||||
group,
|
||||
rclcpp::node_interfaces::get_node_base_interface(node).get(),
|
||||
rclcpp::node_interfaces::get_node_timers_interface(node).get(),
|
||||
autostart);
|
||||
rclcpp::node_interfaces::get_node_timers_interface(node).get());
|
||||
}
|
||||
|
||||
/// Convenience method to create a general timer with node resources.
|
||||
@@ -136,7 +132,6 @@ create_timer(
|
||||
* \param group callback group
|
||||
* \param node_base node base interface
|
||||
* \param node_timers node timer interface
|
||||
* \param autostart defines if the timer should start it's countdown on initialization or not.
|
||||
* \return shared pointer to a generic timer
|
||||
* \throws std::invalid_argument if either clock, node_base or node_timers
|
||||
* are nullptr, or period is negative or too large
|
||||
@@ -149,8 +144,7 @@ create_timer(
|
||||
CallbackT callback,
|
||||
rclcpp::CallbackGroup::SharedPtr group,
|
||||
node_interfaces::NodeBaseInterface * node_base,
|
||||
node_interfaces::NodeTimersInterface * node_timers,
|
||||
bool autostart = true)
|
||||
node_interfaces::NodeTimersInterface * node_timers)
|
||||
{
|
||||
if (clock == nullptr) {
|
||||
throw std::invalid_argument{"clock cannot be null"};
|
||||
@@ -166,7 +160,7 @@ create_timer(
|
||||
|
||||
// Add a new generic timer.
|
||||
auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
|
||||
std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart);
|
||||
std::move(clock), period_ns, std::move(callback), node_base->get_context());
|
||||
node_timers->add_timer(timer, group);
|
||||
return timer;
|
||||
}
|
||||
@@ -193,8 +187,7 @@ create_wall_timer(
|
||||
CallbackT callback,
|
||||
rclcpp::CallbackGroup::SharedPtr group,
|
||||
node_interfaces::NodeBaseInterface * node_base,
|
||||
node_interfaces::NodeTimersInterface * node_timers,
|
||||
bool autostart = true)
|
||||
node_interfaces::NodeTimersInterface * node_timers)
|
||||
{
|
||||
if (node_base == nullptr) {
|
||||
throw std::invalid_argument{"input node_base cannot be null"};
|
||||
@@ -208,7 +201,7 @@ create_wall_timer(
|
||||
|
||||
// Add a new wall timer.
|
||||
auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
|
||||
period_ns, std::move(callback), node_base->get_context(), autostart);
|
||||
period_ns, std::move(callback), node_base->get_context());
|
||||
node_timers->add_timer(timer, group);
|
||||
return timer;
|
||||
}
|
||||
|
||||
@@ -47,11 +47,6 @@ resolve_intra_process_buffer_type(
|
||||
return resolved_buffer_type;
|
||||
}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::IntraProcessBufferType
|
||||
resolve_intra_process_buffer_type(
|
||||
const rclcpp::IntraProcessBufferType buffer_type);
|
||||
|
||||
} // namespace detail
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -18,7 +18,6 @@
|
||||
#include <chrono>
|
||||
|
||||
#include "builtin_interfaces/msg/duration.hpp"
|
||||
#include "builtin_interfaces/msg/time.hpp"
|
||||
#include "rcl/time.h"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -159,14 +158,6 @@ private:
|
||||
Duration() = default;
|
||||
};
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
builtin_interfaces::msg::Time
|
||||
operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
builtin_interfaces::msg::Time
|
||||
operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs);
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DURATION_HPP_
|
||||
|
||||
176
rclcpp/include/rclcpp/dynamic_subscription.hpp
Normal file
176
rclcpp/include/rclcpp/dynamic_subscription.hpp
Normal file
@@ -0,0 +1,176 @@
|
||||
// Copyright 2022 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__DYNAMIC_SUBSCRIPTION_HPP_
|
||||
#define RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
|
||||
|
||||
#include <rosidl_dynamic_typesupport/identifier.h>
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcpputils/shared_library.hpp"
|
||||
|
||||
#include "rclcpp/callback_group.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/serialized_message.hpp"
|
||||
#include "rclcpp/subscription_base.hpp"
|
||||
#include "rclcpp/typesupport_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
/// %Subscription for messages whose type descriptions are obtained at runtime.
|
||||
/**
|
||||
* Since the type is not known at compile time, this is not a template, and the dynamic library
|
||||
* containing type support information has to be identified and loaded based on the type name.
|
||||
*
|
||||
* NOTE(methylDragon): No considerations for intra-process handling are made.
|
||||
*/
|
||||
class DynamicSubscription : public rclcpp::SubscriptionBase
|
||||
{
|
||||
public:
|
||||
// cppcheck-suppress unknownMacro
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(DynamicSubscription)
|
||||
|
||||
template<typename AllocatorT = std::allocator<void>>
|
||||
DynamicSubscription(
|
||||
rclcpp::node_interfaces::NodeBaseInterface * node_base,
|
||||
rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr type_support,
|
||||
const std::string & topic_name,
|
||||
const rclcpp::QoS & qos,
|
||||
std::function<void(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
|
||||
std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
|
||||
)> callback,
|
||||
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
|
||||
bool use_take_dynamic_message = true)
|
||||
: SubscriptionBase(
|
||||
node_base,
|
||||
*(type_support->get_rosidl_message_type_support()),
|
||||
topic_name,
|
||||
options.to_rcl_subscription_options(
|
||||
qos),
|
||||
options.event_callbacks,
|
||||
options.use_default_callbacks,
|
||||
use_take_dynamic_message ? SubscriptionType::DYNAMIC_MESSAGE_DIRECT : SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED), // NOLINT
|
||||
ts_(type_support),
|
||||
callback_(callback),
|
||||
serialization_support_(nullptr),
|
||||
dynamic_message_(nullptr),
|
||||
dynamic_message_type_(nullptr)
|
||||
{
|
||||
if (!type_support) {
|
||||
throw std::runtime_error("DynamicMessageTypeSupport cannot be nullptr!");
|
||||
}
|
||||
|
||||
if (type_support->get_rosidl_message_type_support()->typesupport_identifier !=
|
||||
rosidl_get_dynamic_typesupport_identifier())
|
||||
{
|
||||
throw std::runtime_error(
|
||||
"DynamicSubscription must use dynamic type introspection type support!");
|
||||
}
|
||||
|
||||
serialization_support_ = type_support->get_shared_dynamic_serialization_support();
|
||||
dynamic_message_type_ = type_support->get_shared_dynamic_message_type()->clone_shared();
|
||||
dynamic_message_ = type_support->get_shared_dynamic_message()->clone_shared();
|
||||
}
|
||||
|
||||
// TODO(methylDragon):
|
||||
/// Deferred type description constructor, only usable if the middleware implementation supports
|
||||
/// type discovery
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicSubscription() = default;
|
||||
|
||||
// Same as create_serialized_message() as the subscription is to serialized_messages only
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<void> create_message() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rclcpp::SerializedMessage> create_serialized_message() override;
|
||||
|
||||
/// Cast the message to a rclcpp::SerializedMessage and call the callback.
|
||||
RCLCPP_PUBLIC
|
||||
void handle_message(
|
||||
std::shared_ptr<void> & message, const rclcpp::MessageInfo & message_info) override;
|
||||
|
||||
/// Handle dispatching rclcpp::SerializedMessage to user callback.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
handle_serialized_message(
|
||||
const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
|
||||
const rclcpp::MessageInfo & message_info) override;
|
||||
|
||||
/// This function is currently not implemented.
|
||||
RCLCPP_PUBLIC
|
||||
void handle_loaned_message(
|
||||
void * loaned_message, const rclcpp::MessageInfo & message_info) override;
|
||||
|
||||
// Same as return_serialized_message() as the subscription is to serialized_messages only
|
||||
RCLCPP_PUBLIC
|
||||
void return_message(std::shared_ptr<void> & message) override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
|
||||
|
||||
|
||||
// DYNAMIC TYPE ==================================================================================
|
||||
// TODO(methylDragon): Reorder later
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
|
||||
get_shared_dynamic_message_type() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void return_dynamic_message(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void handle_dynamic_message(
|
||||
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
|
||||
const rclcpp::MessageInfo & message_info) override;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicSubscription)
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr ts_;
|
||||
std::function<void(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
|
||||
std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
|
||||
)> callback_;
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr dynamic_message_;
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr dynamic_message_type_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_SUBSCRIPTION_HPP_
|
||||
@@ -0,0 +1,327 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
|
||||
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
|
||||
#endif
|
||||
|
||||
|
||||
#define __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
|
||||
template<> \
|
||||
ValueT \
|
||||
DynamicMessage::get_value<ValueT>(rosidl_dynamic_typesupport_member_id_t id) \
|
||||
{ \
|
||||
ValueT out; \
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_ ## FunctionT ## _value( \
|
||||
rosidl_dynamic_data_.get(), id, &out); \
|
||||
return out; \
|
||||
}
|
||||
|
||||
#define __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
|
||||
template<> \
|
||||
ValueT \
|
||||
DynamicMessage::get_value<ValueT>(const std::string & name) \
|
||||
{ \
|
||||
return get_value<ValueT>(get_member_id(name)); \
|
||||
}
|
||||
|
||||
#define __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
|
||||
template<> \
|
||||
void \
|
||||
DynamicMessage::set_value<ValueT>(rosidl_dynamic_typesupport_member_id_t id, ValueT value) \
|
||||
{ \
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_ ## FunctionT ## _value( \
|
||||
rosidl_dynamic_data_.get(), id, value); \
|
||||
}
|
||||
|
||||
#define __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
|
||||
template<> \
|
||||
void \
|
||||
DynamicMessage::set_value<ValueT>(const std::string & name, ValueT value) \
|
||||
{ \
|
||||
set_value<ValueT>(get_member_id(name), value); \
|
||||
}
|
||||
|
||||
#define __DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT) \
|
||||
template<> \
|
||||
rosidl_dynamic_typesupport_member_id_t \
|
||||
DynamicMessage::insert_value<ValueT>(ValueT value) \
|
||||
{ \
|
||||
rosidl_dynamic_typesupport_member_id_t out; \
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_ ## FunctionT ## _value( \
|
||||
rosidl_dynamic_data_.get(), value, &out); \
|
||||
return out; \
|
||||
}
|
||||
|
||||
#define DYNAMIC_MESSAGE_DEFINITIONS(ValueT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN(ValueT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN(ValueT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN(ValueT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_INSERT_VALUE(ValueT, FunctionT)
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/**
|
||||
* Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
|
||||
* https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
|
||||
*
|
||||
* Explicitly:
|
||||
* - Basic types: bool, byte, char
|
||||
* - Float types: float, double
|
||||
* - Int types: int8_t, int16_t, int32_t, int64_t
|
||||
* - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
|
||||
* - String types: std::string, std::u16string
|
||||
*/
|
||||
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(bool, bool);
|
||||
// DYNAMIC_MESSAGE_DEFINITIONS(std::byte, byte);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(char, char);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(float, float32);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(double, float64);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(int8_t, int8);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(int16_t, int16);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(int32_t, int32);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(int64_t, int64);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(uint8_t, uint8);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(uint16_t, uint16);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(uint32_t, uint32);
|
||||
DYNAMIC_MESSAGE_DEFINITIONS(uint64_t, uint64);
|
||||
// DYNAMIC_MESSAGE_DEFINITIONS(std::string, std::string);
|
||||
// DYNAMIC_MESSAGE_DEFINITIONS(std::u16string, std::u16string);
|
||||
|
||||
// Byte and String getters have a different implementation and are defined below
|
||||
|
||||
|
||||
// BYTE ============================================================================================
|
||||
template<>
|
||||
std::byte
|
||||
DynamicMessage::get_value<std::byte>(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
unsigned char out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_byte_value(get_rosidl_dynamic_data(), id, &out);
|
||||
return static_cast<std::byte>(out);
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
std::byte
|
||||
DynamicMessage::get_value<std::byte>(const std::string & name)
|
||||
{
|
||||
return get_value<std::byte>(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
void
|
||||
DynamicMessage::set_value<std::byte>(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::byte value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_byte_value(
|
||||
rosidl_dynamic_data_.get(), id, static_cast<unsigned char>(value));
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
void
|
||||
DynamicMessage::set_value<std::byte>(const std::string & name, const std::byte value)
|
||||
{
|
||||
set_value<std::byte>(get_member_id(name), value);
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_value<std::byte>(const std::byte value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_byte_value(
|
||||
rosidl_dynamic_data_.get(), static_cast<unsigned char>(value), &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// STRINGS =========================================================================================
|
||||
template<>
|
||||
std::string
|
||||
DynamicMessage::get_value<std::string>(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
size_t buf_length;
|
||||
char * buf = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_string_value(
|
||||
get_rosidl_dynamic_data(), id, &buf, &buf_length);
|
||||
auto out = std::string(buf, buf_length);
|
||||
delete buf;
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
std::u16string
|
||||
DynamicMessage::get_value<std::u16string>(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
size_t buf_length;
|
||||
char16_t * buf = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_wstring_value(
|
||||
get_rosidl_dynamic_data(), id, &buf, &buf_length);
|
||||
auto out = std::u16string(buf, buf_length);
|
||||
delete buf;
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
std::string
|
||||
DynamicMessage::get_value<std::string>(const std::string & name)
|
||||
{
|
||||
return get_value<std::string>(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
std::u16string
|
||||
DynamicMessage::get_value<std::u16string>(const std::string & name)
|
||||
{
|
||||
return get_value<std::u16string>(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
void
|
||||
DynamicMessage::set_value<std::string>(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_string_value(
|
||||
rosidl_dynamic_data_.get(), id, value.c_str(), value.size());
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
void
|
||||
DynamicMessage::set_value<std::u16string>(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::u16string value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_wstring_value(
|
||||
rosidl_dynamic_data_.get(), id, value.c_str(), value.size());
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
void
|
||||
DynamicMessage::set_value<std::string>(const std::string & name, const std::string value)
|
||||
{
|
||||
set_value<std::string>(get_member_id(name), value);
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
void
|
||||
DynamicMessage::set_value<std::u16string>(const std::string & name, const std::u16string value)
|
||||
{
|
||||
set_value<std::u16string>(get_member_id(name), value);
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_value<std::string>(const std::string value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_string_value(
|
||||
rosidl_dynamic_data_.get(), value.c_str(), value.size(), &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
template<>
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_value<std::u16string>(const std::u16string value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_wstring_value(
|
||||
rosidl_dynamic_data_.get(), value.c_str(), value.size(), &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// THROW FOR UNSUPPORTED TYPES =====================================================================
|
||||
template<typename ValueT>
|
||||
ValueT
|
||||
DynamicMessage::get_value(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
template<typename ValueT>
|
||||
ValueT
|
||||
DynamicMessage::get_value(const std::string & name)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError("get_value is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
template<typename ValueT>
|
||||
void
|
||||
DynamicMessage::set_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, ValueT value)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
template<typename ValueT>
|
||||
void
|
||||
DynamicMessage::set_value(const std::string & name, ValueT value)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError("set_value is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
template<typename ValueT>
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_value(ValueT value)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError("insert_value is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_ID_FN
|
||||
#undef __DYNAMIC_MESSAGE_GET_VALUE_BY_NAME_FN
|
||||
#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_ID_FN
|
||||
#undef __DYNAMIC_MESSAGE_SET_VALUE_BY_NAME_FN
|
||||
#undef __DYNAMIC_MESSAGE_INSERT_VALUE
|
||||
#undef DYNAMIC_MESSAGE_DEFINITIONS
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
|
||||
@@ -0,0 +1,189 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
|
||||
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
|
||||
#endif
|
||||
|
||||
|
||||
#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
|
||||
template<> \
|
||||
void \
|
||||
DynamicMessageTypeBuilder::add_member<MemberT>( \
|
||||
rosidl_dynamic_typesupport_member_id_t id, \
|
||||
const std::string & name, \
|
||||
const std::string & default_value) \
|
||||
{ \
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _member( \
|
||||
rosidl_dynamic_type_builder_.get(), \
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \
|
||||
}
|
||||
|
||||
#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
|
||||
template<> \
|
||||
void \
|
||||
DynamicMessageTypeBuilder::add_array_member<MemberT>( \
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, \
|
||||
size_t array_length, \
|
||||
const std::string & default_value) \
|
||||
{ \
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _array_member( \
|
||||
rosidl_dynamic_type_builder_.get(), \
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \
|
||||
array_length); \
|
||||
}
|
||||
|
||||
#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
|
||||
template<> \
|
||||
void \
|
||||
DynamicMessageTypeBuilder::add_unbounded_sequence_member<MemberT>( \
|
||||
rosidl_dynamic_typesupport_member_id_t id, \
|
||||
const std::string & name, \
|
||||
const std::string & default_value) \
|
||||
{ \
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## \
|
||||
_unbounded_sequence_member( \
|
||||
rosidl_dynamic_type_builder_.get(), \
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size()); \
|
||||
}
|
||||
|
||||
#define __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
|
||||
template<> \
|
||||
void \
|
||||
DynamicMessageTypeBuilder::add_bounded_sequence_member<MemberT>( \
|
||||
rosidl_dynamic_typesupport_member_id_t id, \
|
||||
const std::string & name, \
|
||||
size_t sequence_bound, \
|
||||
const std::string & default_value) \
|
||||
{ \
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_ ## FunctionT ## _bounded_sequence_member( \
|
||||
rosidl_dynamic_type_builder_.get(), \
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(), \
|
||||
sequence_bound); \
|
||||
}
|
||||
|
||||
#define DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(MemberT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN(MemberT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN(MemberT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
|
||||
__DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN(MemberT, FunctionT) \
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/**
|
||||
* Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
|
||||
* https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
|
||||
*
|
||||
* Explicitly:
|
||||
* - Basic types: bool, byte, char
|
||||
* - Float types: float, double
|
||||
* - Int types: int8_t, int16_t, int32_t, int64_t
|
||||
* - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
|
||||
* - String types: std::string, std::u16string
|
||||
*/
|
||||
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(bool, bool);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::byte, byte);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(char, char);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(float, float32);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(double, float64);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int8_t, int8);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int16_t, int16);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int32_t, int32);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(int64_t, int64);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint8_t, uint8);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint16_t, uint16);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint32_t, uint32);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(uint64_t, uint64);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::string, string);
|
||||
DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS(std::u16string, wstring);
|
||||
|
||||
|
||||
// THROW FOR UNSUPPORTED TYPES =====================================================================
|
||||
template<typename MemberT>
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id,
|
||||
const std::string & name,
|
||||
const std::string & default_value)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"add_member is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
template<typename MemberT>
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id,
|
||||
const std::string & name,
|
||||
size_t array_length, const std::string & default_value)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"add_array_member is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
template<typename MemberT>
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id,
|
||||
const std::string & name,
|
||||
const std::string & default_value)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"add_unbounded_sequence_member is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
template<typename MemberT>
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id,
|
||||
const std::string & name,
|
||||
size_t sequence_bound,
|
||||
const std::string & default_value)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"add_bounded_sequence_member is not implemented for input type");
|
||||
}
|
||||
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_MEMBER_FN
|
||||
#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_ARRAY_MEMBER_FN
|
||||
#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_UNBOUNDED_SEQUENCE_MEMBER_FN
|
||||
#undef __DYNAMIC_MESSAGE_TYPE_BUILDER_ADD_BOUNDED_SEQUENCE_MEMBER_FN
|
||||
#undef DYNAMIC_MESSAGE_TYPE_BUILDER_DEFINITIONS
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
|
||||
@@ -15,7 +15,6 @@
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rcl/types.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
@@ -27,21 +26,376 @@
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t
|
||||
/// STUBBED OUT
|
||||
|
||||
class DynamicMessageType;
|
||||
class DynamicMessageTypeBuilder;
|
||||
|
||||
/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t *
|
||||
/**
|
||||
* This class:
|
||||
* - Manages the lifetime of the raw pointer.
|
||||
* - Exposes getter methods to get the raw pointer and shared pointers
|
||||
* - Exposes the underlying serialization support API
|
||||
*
|
||||
* Ownership:
|
||||
* - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed
|
||||
* DynamicSerializationSupport. So it cannot outlive the DynamicSerializationSupport.
|
||||
* - The DynamicSerializationSupport's rosidl_dynamic_typesupport_serialization_support_t pointer
|
||||
* must point to the same location in memory as the stored raw pointer!
|
||||
*
|
||||
* Note: This class is meant to map to the lower level rosidl_dynamic_typesupport_dynamic_data_t,
|
||||
* even though rosidl_dynamic_typesupport_dynamic_data_t is equivalent to
|
||||
* rosidl_dynamic_typesupport_dynamic_data_t, exposing the fundamental methods available to
|
||||
* rosidl_dynamic_typesupport_dynamic_data_t, allowing the user to access the data's fields.
|
||||
*/
|
||||
class DynamicMessage : public std::enable_shared_from_this<DynamicMessage>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage)
|
||||
|
||||
// CONSTRUCTION ==================================================================================
|
||||
// Most constructors require a passed in DynamicSerializationSupport::SharedPtr, to extend the
|
||||
// lifetime of the serialization support (if the constructor cannot otherwise get it from args).
|
||||
//
|
||||
// In cases where a dynamic data pointer is passed, the serialization support composed by
|
||||
// the data should be the exact same object managed by the DynamicSerializationSupport,
|
||||
// otherwise the lifetime management will not work properly.
|
||||
|
||||
/// Construct a new DynamicMessage with the provided dynamic type builder
|
||||
RCLCPP_PUBLIC
|
||||
explicit DynamicMessage(std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder);
|
||||
|
||||
/// Construct a new DynamicMessage with the provided dynamic type
|
||||
RCLCPP_PUBLIC
|
||||
explicit DynamicMessage(std::shared_ptr<DynamicMessageType> dynamic_type);
|
||||
|
||||
/// Assume ownership of raw pointer
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data);
|
||||
|
||||
/// Copy shared pointer
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> rosidl_dynamic_data);
|
||||
|
||||
/// Loaning constructor
|
||||
/// Must only be called with raw ptr obtained from loaning!
|
||||
// NOTE(methylDragon): I'd put this in protected, but I need this exposed to
|
||||
// enable_shared_from_this...
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage(
|
||||
DynamicMessage::SharedPtr parent_data,
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_loaned_data);
|
||||
|
||||
// NOTE(methylDragon): Deliberately no constructor from description to nudge users towards using
|
||||
// construction from dynamic type/builder, which is more efficient
|
||||
|
||||
/// Copy constructor
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage(const DynamicMessage & other);
|
||||
|
||||
/// Move constructor
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage(DynamicMessage && other) noexcept;
|
||||
|
||||
/// Copy assignment
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage & operator=(const DynamicMessage & other);
|
||||
|
||||
/// Move assignment
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage & operator=(DynamicMessage && other) noexcept;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessage();
|
||||
|
||||
|
||||
// GETTERS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_library_identifier() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_name() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_dynamic_data_t *
|
||||
get_rosidl_dynamic_data();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_dynamic_typesupport_dynamic_data_t *
|
||||
get_rosidl_dynamic_data() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
|
||||
get_shared_rosidl_dynamic_data();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_data_t>
|
||||
get_shared_rosidl_dynamic_data() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
get_shared_dynamic_serialization_support() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_item_count() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
get_member_id(size_t index) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
get_member_id(const std::string & name) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
get_array_index(size_t index) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
get_array_index(const std::string & name) const;
|
||||
|
||||
|
||||
// METHODS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage
|
||||
clone() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
clone_shared() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage
|
||||
init_from_type(DynamicMessageType & type) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
init_from_type_shared(DynamicMessageType & type) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
equals(const DynamicMessage & other) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
loan_value(rosidl_dynamic_typesupport_member_id_t id);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
loan_value(const std::string & name);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_all_values();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_nonkey_values();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_value(rosidl_dynamic_typesupport_member_id_t id);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_value(const std::string & name);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_sequence();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_sequence_data();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
serialize(rcl_serialized_message_t * buffer);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
deserialize(rcl_serialized_message_t * buffer);
|
||||
|
||||
|
||||
// MEMBER ACCESS TEMPLATES =======================================================================
|
||||
/**
|
||||
* Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
|
||||
* https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
|
||||
*
|
||||
* Explicitly:
|
||||
* - Basic types: bool, byte, char
|
||||
* - Float types: float, double
|
||||
* - Int types: int8_t, int16_t, int32_t, int64_t
|
||||
* - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
|
||||
* - String types: std::string, std::u16string
|
||||
*/
|
||||
|
||||
template<typename ValueT>
|
||||
ValueT
|
||||
get_value(rosidl_dynamic_typesupport_member_id_t id);
|
||||
|
||||
template<typename ValueT>
|
||||
ValueT
|
||||
get_value(const std::string & name);
|
||||
|
||||
template<typename ValueT>
|
||||
void
|
||||
set_value(rosidl_dynamic_typesupport_member_id_t id, ValueT value);
|
||||
|
||||
template<typename ValueT>
|
||||
void
|
||||
set_value(const std::string & name, ValueT value);
|
||||
|
||||
template<typename ValueT>
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_value(ValueT value);
|
||||
|
||||
|
||||
// FIXED STRING MEMBER ACCESS ====================================================================
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_fixed_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_fixed_string_value(const std::string & name, size_t string_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::u16string
|
||||
get_fixed_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::u16string
|
||||
get_fixed_wstring_value(const std::string & name, size_t wstring_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_fixed_string_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_fixed_string_value(const std::string & name, const std::string value, size_t string_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_fixed_wstring_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_fixed_wstring_value(
|
||||
const std::string & name, const std::u16string value, size_t wstring_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_fixed_string_value(const std::string value, size_t string_length);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_fixed_wstring_value(const std::u16string value, size_t wstring_length);
|
||||
|
||||
|
||||
// BOUNDED STRING MEMBER ACCESS ==================================================================
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_bounded_string_value(rosidl_dynamic_typesupport_member_id_t id, size_t string_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_bounded_string_value(const std::string & name, size_t string_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::u16string
|
||||
get_bounded_wstring_value(rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::u16string
|
||||
get_bounded_wstring_value(const std::string & name, size_t wstring_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_bounded_string_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_bounded_string_value(const std::string & name, const std::string value, size_t string_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_bounded_wstring_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_bounded_wstring_value(
|
||||
const std::string & name, const std::u16string value, size_t wstring_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_bounded_string_value(const std::string value, size_t string_bound);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound);
|
||||
|
||||
|
||||
// NESTED MEMBER ACCESS ==========================================================================
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage
|
||||
get_complex_value(rosidl_dynamic_typesupport_member_id_t id);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage
|
||||
get_complex_value(const std::string & name);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
get_complex_value_shared(const std::string & name);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_complex_value(rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_complex_value(const std::string & name, DynamicMessage & value);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_complex_value_copy(const DynamicMessage & value);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
insert_complex_value(DynamicMessage & value);
|
||||
|
||||
protected:
|
||||
// NOTE(methylDragon):
|
||||
// This is just here to extend the lifetime of the serialization support
|
||||
@@ -51,19 +405,25 @@ protected:
|
||||
// DynamicSerializationSupport
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data_;
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> rosidl_dynamic_data_;
|
||||
|
||||
bool is_loaned_;
|
||||
|
||||
// Used for returning the loaned value, and lifetime management
|
||||
DynamicMessage::SharedPtr parent_data_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessage)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
match_serialization_support_(
|
||||
const DynamicSerializationSupport & serialization_support,
|
||||
const rosidl_dynamic_typesupport_dynamic_data_t & dynamic_data);
|
||||
};
|
||||
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
@@ -15,7 +15,6 @@
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
@@ -25,21 +24,160 @@
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t`
|
||||
/// STUBBED OUT
|
||||
|
||||
class DynamicMessage;
|
||||
class DynamicMessageTypeBuilder;
|
||||
|
||||
/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t *`
|
||||
/**
|
||||
* This class:
|
||||
* - Manages the lifetime of the raw pointer.
|
||||
* - Exposes getter methods to get the raw pointer and shared pointers
|
||||
* - Exposes the underlying serialization support API
|
||||
*
|
||||
* Ownership:
|
||||
* - This class borrows the `rosidl_dynamic_typesupport_serialization_support_t` stored in the
|
||||
* passed `DynamicSerializationSupport`.
|
||||
* So it cannot outlive the `DynamicSerializationSupport`.
|
||||
* - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t`
|
||||
* pointer must point to the same location in memory as the stored raw pointer!
|
||||
*
|
||||
* This class is meant to map to the lower level `rosidl_dynamic_typesupport_dynamic_type_t`,
|
||||
* which can be constructed via `DynamicMessageTypeBuilder`, which maps to
|
||||
* `rosidl_dynamic_typesupport_dynamic_type_builder_t`.
|
||||
*
|
||||
* The usual method of obtaining a `DynamicMessageType` is through construction of
|
||||
* `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then
|
||||
* taking ownership of its contents. But `DynamicMessageTypeBuilder` can also be used to obtain
|
||||
* `DynamicMessageType` by constructing it bottom-up instead, since it exposes the lower_level
|
||||
* rosidl methods.
|
||||
*/
|
||||
class DynamicMessageType : public std::enable_shared_from_this<DynamicMessageType>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType)
|
||||
|
||||
// CONSTRUCTION ==================================================================================
|
||||
// Most constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the
|
||||
// lifetime of the serialization support (if the constructor cannot otherwise get it from args).
|
||||
//
|
||||
// In cases where a dynamic type pointer is passed, the serialization support composed by
|
||||
// the type should be the exact same object managed by the `DynamicSerializationSupport`,
|
||||
// otherwise the lifetime management will not work properly.
|
||||
|
||||
/// Construct a new `DynamicMessageType` with the provided dynamic type builder
|
||||
RCLCPP_PUBLIC
|
||||
explicit DynamicMessageType(std::shared_ptr<DynamicMessageTypeBuilder> dynamic_type_builder);
|
||||
|
||||
/// Assume ownership of raw pointer
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type);
|
||||
|
||||
/// Copy shared pointer
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> rosidl_dynamic_type);
|
||||
|
||||
/// From description
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description);
|
||||
|
||||
/// Copy constructor
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType(const DynamicMessageType & other);
|
||||
|
||||
/// Move constructor
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType(DynamicMessageType && other) noexcept;
|
||||
|
||||
/// Copy assignment
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType & operator=(const DynamicMessageType & other);
|
||||
|
||||
/// Move assignment
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType & operator=(DynamicMessageType && other) noexcept;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessageType();
|
||||
|
||||
/// Swaps the serialization support if `serialization_support` is populated
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
init_from_description(
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description,
|
||||
DynamicSerializationSupport::SharedPtr serialization_support = nullptr);
|
||||
|
||||
// GETTERS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_library_identifier() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_name() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_member_count() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_dynamic_type_t *
|
||||
get_rosidl_dynamic_type();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_dynamic_typesupport_dynamic_type_t *
|
||||
get_rosidl_dynamic_type() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
|
||||
get_shared_rosidl_dynamic_type();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_t>
|
||||
get_shared_rosidl_dynamic_type() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
get_shared_dynamic_serialization_support() const;
|
||||
|
||||
|
||||
// METHODS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType
|
||||
clone() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType::SharedPtr
|
||||
clone_shared() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
equals(const DynamicMessageType & other) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage
|
||||
build_dynamic_message();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<DynamicMessage>
|
||||
build_dynamic_message_shared();
|
||||
|
||||
protected:
|
||||
// NOTE(methylDragon):
|
||||
// This is just here to extend the lifetime of the serialization support
|
||||
@@ -49,15 +187,20 @@ protected:
|
||||
// `DynamicSerializationSupport`
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type_;
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> rosidl_dynamic_type_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessageType)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
match_serialization_support_(
|
||||
const DynamicSerializationSupport & serialization_support,
|
||||
const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type);
|
||||
};
|
||||
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
@@ -15,7 +15,6 @@
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
@@ -25,21 +24,360 @@
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
class DynamicMessage;
|
||||
class DynamicMessageType;
|
||||
|
||||
/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *`
|
||||
/// STUBBED OUT
|
||||
/**
|
||||
* This class:
|
||||
* - Manages the lifetime of the raw pointer.
|
||||
* - Exposes getter methods to get the raw pointer and shared pointers
|
||||
* - Exposes the underlying serialization support API
|
||||
*
|
||||
* Ownership:
|
||||
* - This class borrows the rosidl_dynamic_typesupport_serialization_support_t stored in the passed
|
||||
* `DynamicSerializationSupport`.
|
||||
* So it cannot outlive the `DynamicSerializationSupport`.
|
||||
* - The `DynamicSerializationSupport`'s `rosidl_dynamic_typesupport_serialization_support_t`
|
||||
* pointer must point to the same location in memory as the stored raw pointer!
|
||||
*
|
||||
* This class is meant to map to rosidl_dynamic_typesupport_dynamic_type_builder_t, facilitating the
|
||||
* construction of dynamic types bottom-up in the C++ layer.
|
||||
*
|
||||
* The usual method of obtaining a `DynamicMessageType` is through construction of
|
||||
* `rosidl_message_type_support_t` via `rcl_dynamic_message_type_support_handle_create()`, then
|
||||
* taking ownership of its contents.
|
||||
* But `DynamicMessageTypeBuilder` can also be used to obtain `DynamicMessageType` by constructing
|
||||
* it bottom-up instead, since it exposes the lower_level rosidl methods.
|
||||
*/
|
||||
class DynamicMessageTypeBuilder : public std::enable_shared_from_this<DynamicMessageTypeBuilder>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder)
|
||||
|
||||
// CONSTRUCTION ==================================================================================
|
||||
// All constructors require a passed in `DynamicSerializationSupport::SharedPtr`, to extend the
|
||||
// lifetime of the serialization support.
|
||||
//
|
||||
// In cases where a dynamic type builder pointer is passed, the serialization support composed by
|
||||
// the builder should be the exact same object managed by the `DynamicSerializationSupport`,
|
||||
// otherwise the lifetime management will not work properly.
|
||||
|
||||
/// Construct a new `DynamicMessageTypeBuilder` with the provided serialization support
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const std::string & name);
|
||||
|
||||
/// Assume ownership of raw pointer
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * dynamic_type_builder);
|
||||
|
||||
/// Copy shared pointer
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t> dynamic_type_builder);
|
||||
|
||||
/// Copy constructor
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder(const DynamicMessageTypeBuilder & other);
|
||||
|
||||
/// Move constructor
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept;
|
||||
|
||||
/// Copy assignment
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder & operator=(const DynamicMessageTypeBuilder & other);
|
||||
|
||||
/// Move assignment
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder & operator=(DynamicMessageTypeBuilder && other) noexcept;
|
||||
|
||||
/// From description
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessageTypeBuilder();
|
||||
|
||||
/// Swaps the serialization support if serialization_support is populated
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
init_from_description(
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description,
|
||||
DynamicSerializationSupport::SharedPtr serialization_support = nullptr);
|
||||
|
||||
|
||||
// GETTERS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_library_identifier() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_name() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t *
|
||||
get_rosidl_dynamic_type_builder();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_dynamic_typesupport_dynamic_type_builder_t *
|
||||
get_rosidl_dynamic_type_builder() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>
|
||||
get_shared_rosidl_dynamic_type_builder();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_builder_t>
|
||||
get_shared_rosidl_dynamic_type_builder() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
get_shared_dynamic_serialization_support() const;
|
||||
|
||||
|
||||
// METHODS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_name(const std::string & name);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder
|
||||
clone() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder::SharedPtr
|
||||
clone_shared() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage
|
||||
build_dynamic_message();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
build_dynamic_message_shared();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType
|
||||
build_dynamic_message_type();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType::SharedPtr
|
||||
build_dynamic_message_type_shared();
|
||||
|
||||
|
||||
// ADD MEMBERS TEMPLATES =========================================================================
|
||||
/**
|
||||
* Since we're in a ROS layer, these should support all ROS interface C++ types as found in:
|
||||
* https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html
|
||||
*
|
||||
* Explicitly:
|
||||
* - Basic types: bool, byte, char
|
||||
* - Float types: float, double
|
||||
* - Int types: int8_t, int16_t, int32_t, int64_t
|
||||
* - Unsigned int types: uint8_t, uint16_t, uint32_t, uint64_t
|
||||
* - String types: std::string, std::u16string
|
||||
*/
|
||||
|
||||
template<typename MemberT>
|
||||
void
|
||||
add_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
const std::string & default_value = "");
|
||||
|
||||
template<typename MemberT>
|
||||
void
|
||||
add_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t array_length,
|
||||
const std::string & default_value = "");
|
||||
|
||||
template<typename MemberT>
|
||||
void
|
||||
add_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
const std::string & default_value = "");
|
||||
|
||||
template<typename MemberT>
|
||||
void
|
||||
add_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t sequence_bound,
|
||||
const std::string & default_value = "");
|
||||
|
||||
|
||||
// ADD FIXED STRING MEMBERS ======================================================================
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_string_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_wstring_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_string_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_length, size_t array_length, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_wstring_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_length, size_t array_length, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_string_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_wstring_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_string_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_length, size_t sequence_bound, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_fixed_wstring_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_length, size_t sequence_bound, const std::string & default_value = "");
|
||||
|
||||
|
||||
// ADD BOUNDED STRING MEMBERS ====================================================================
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_string_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_wstring_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_string_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_bound, size_t array_length, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_wstring_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_bound, size_t array_length, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_string_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_wstring_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_string_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_bound, size_t sequence_bound, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_bounded_wstring_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_bound, size_t sequence_bound, const std::string & default_value = "");
|
||||
|
||||
|
||||
// ADD NESTED MEMBERS ============================================================================
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, size_t array_length, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, size_t sequence_bound,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_array_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, size_t array_length,
|
||||
const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_unbounded_sequence_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value = "");
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_complex_bounded_sequence_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound,
|
||||
const std::string & default_value = "");
|
||||
|
||||
protected:
|
||||
// NOTE(methylDragon):
|
||||
// This is just here to extend the lifetime of the serialization support
|
||||
@@ -49,15 +387,26 @@ protected:
|
||||
// `DynamicSerializationSupport`
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder_;
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t> rosidl_dynamic_type_builder_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessageTypeBuilder)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
init_from_serialization_support_(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const std::string & name);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
match_serialization_support_(
|
||||
const DynamicSerializationSupport & serialization_support,
|
||||
const rosidl_dynamic_typesupport_dynamic_type_builder_t & dynamic_type_builder);
|
||||
};
|
||||
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
@@ -15,8 +15,6 @@
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
|
||||
#include <rosidl_dynamic_typesupport/dynamic_message_type_support_struct.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
#include <rosidl_runtime_c/message_type_support_struct.h>
|
||||
@@ -32,35 +30,175 @@
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for `rosidl_message_type_support_t` containing managed
|
||||
/// STUBBED OUT
|
||||
/// Utility wrapper class for `rosidl_message_type_support_t *` containing managed
|
||||
/// instances of the typesupport handle impl.
|
||||
/**
|
||||
*
|
||||
* NOTE: This class is the recommended way to obtain the dynamic message type
|
||||
* support struct, instead of `rcl_dynamic_message_type_support_handle_create()`,
|
||||
* because this class will manage the lifetimes for you.
|
||||
*
|
||||
* Do NOT call rcl_dynamic_message_type_support_handle_destroy!!
|
||||
*
|
||||
* This class:
|
||||
* - Manages the lifetime of the raw pointer.
|
||||
* - Exposes getter methods to get the raw pointer and shared pointers
|
||||
* - Stores shared pointers to wrapper classes that expose the underlying
|
||||
* serialization support API
|
||||
*
|
||||
* Ownership:
|
||||
* - This class, similarly to the `rosidl_dynamic_typesupport_serialization_support_t`, must outlive
|
||||
* all downstream usages of the serialization support.
|
||||
*/
|
||||
class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMessageTypeSupport>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport)
|
||||
|
||||
// CONSTRUCTION ==================================================================================
|
||||
/// From description
|
||||
/// Does NOT take ownership of the description (copies instead.)
|
||||
/// Constructs type support top-down (calling `rcl_dynamic_message_type_support_handle_create()`)
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeSupport(
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description,
|
||||
const std::string & serialization_library_name = "");
|
||||
|
||||
/// From description, for provided serialization support
|
||||
/// Does NOT take ownership of the description (copies instead.)
|
||||
/// Constructs type support top-down (calling
|
||||
/// `rosidl_dynamic_message_type_support_handle_create()`)
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeSupport(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description);
|
||||
|
||||
/// Assume ownership of managed types
|
||||
/// Does NOT take ownership of the description (copies instead.)
|
||||
///
|
||||
/// The serialization support used to construct all managed SharedPtrs must match.
|
||||
/// The structure of the provided `description` must match the `dynamic_message_type`
|
||||
/// The structure of the provided `dynamic_message_type` must match the `dynamic_message
|
||||
///
|
||||
/// In this case, the user would have constructed the type support bototm-up (by creating the
|
||||
/// respective dynamic members.)
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeSupport(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
DynamicMessageType::SharedPtr dynamic_message_type,
|
||||
DynamicMessage::SharedPtr dynamic_message,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessageTypeSupport();
|
||||
|
||||
|
||||
// GETTERS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_library_identifier() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_message_type_support_t *
|
||||
get_rosidl_message_type_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_message_type_support_t *
|
||||
get_rosidl_message_type_support() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rosidl_message_type_support_t>
|
||||
get_shared_rosidl_message_type_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<const rosidl_message_type_support_t>
|
||||
get_shared_rosidl_message_type_support() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_runtime_c__type_description__TypeDescription *
|
||||
get_rosidl_runtime_c_type_description();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_runtime_c__type_description__TypeDescription *
|
||||
get_rosidl_runtime_c_type_description() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rosidl_runtime_c__type_description__TypeDescription>
|
||||
get_shared_rosidl_runtime_c_type_description();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
|
||||
get_shared_rosidl_runtime_c_type_description() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
get_shared_dynamic_serialization_support() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType::SharedPtr
|
||||
get_shared_dynamic_message_type();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType::ConstSharedPtr
|
||||
get_shared_dynamic_message_type() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::SharedPtr
|
||||
get_shared_dynamic_message();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage::ConstSharedPtr
|
||||
get_shared_dynamic_message() const;
|
||||
|
||||
protected:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport)
|
||||
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
DynamicMessageType::SharedPtr dynamic_message_type_;
|
||||
DynamicMessage::SharedPtr dynamic_message_;
|
||||
std::shared_ptr<rosidl_runtime_c__type_description__TypeDescription> description_;
|
||||
|
||||
rosidl_message_type_support_t rosidl_message_type_support_;
|
||||
std::shared_ptr<rosidl_message_type_support_t> rosidl_message_type_support_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeSupport();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
manage_description_(rosidl_runtime_c__type_description__TypeDescription * description);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
init_dynamic_message_type_(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const rosidl_runtime_c__type_description__TypeDescription * description);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
init_dynamic_message_(DynamicMessageType::SharedPtr dynamic_type);
|
||||
|
||||
// By aggregation
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
init_rosidl_message_type_support_(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
DynamicMessageType::SharedPtr dynamic_message_type,
|
||||
DynamicMessage::SharedPtr dynamic_message,
|
||||
rosidl_runtime_c__type_description__TypeDescription * description);
|
||||
};
|
||||
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
@@ -15,8 +15,6 @@
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
@@ -25,35 +23,86 @@
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t
|
||||
/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t *
|
||||
/**
|
||||
* This class:
|
||||
* - Manages the lifetime of the raw pointer.
|
||||
* - Exposes getter methods to get the raw pointer and shared pointers
|
||||
* - Exposes the underlying serialization support API
|
||||
*
|
||||
* Ownership:
|
||||
* - This class, similarly to the rosidl_dynamic_typesupport_serialization_support_t, must outlive
|
||||
* all downstream usages of the serialization support.
|
||||
*/
|
||||
class DynamicSerializationSupport : public std::enable_shared_from_this<DynamicSerializationSupport>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport)
|
||||
|
||||
// CONSTRUCTION ==================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator());
|
||||
DynamicSerializationSupport();
|
||||
|
||||
/// Get the rmw middleware implementation specific serialization support (configured by name)
|
||||
RCLCPP_PUBLIC
|
||||
explicit DynamicSerializationSupport(const std::string & serialization_library_name);
|
||||
|
||||
/// Assume ownership of raw pointer
|
||||
RCLCPP_PUBLIC
|
||||
explicit DynamicSerializationSupport(
|
||||
rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support);
|
||||
|
||||
/// Copy shared pointer
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport(
|
||||
const std::string & serialization_library_name,
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator());
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> serialization_support);
|
||||
|
||||
/// Move constructor
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport(DynamicSerializationSupport && other) noexcept;
|
||||
|
||||
/// Move assignment
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport & operator=(DynamicSerializationSupport && other) noexcept;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicSerializationSupport();
|
||||
|
||||
protected:
|
||||
rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_;
|
||||
|
||||
private:
|
||||
// GETTERS =======================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
const std::string
|
||||
get_library_identifier() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_dynamic_typesupport_serialization_support_t *
|
||||
get_rosidl_serialization_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_dynamic_typesupport_serialization_support_t *
|
||||
get_rosidl_serialization_support() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>
|
||||
get_shared_rosidl_serialization_support();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_serialization_support_t>
|
||||
get_shared_rosidl_serialization_support() const;
|
||||
|
||||
protected:
|
||||
RCLCPP_DISABLE_COPY(DynamicSerializationSupport)
|
||||
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> rosidl_serialization_support_;
|
||||
};
|
||||
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
@@ -260,16 +260,6 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
~EventHandler()
|
||||
{
|
||||
// Since the rmw event listener holds a reference to the
|
||||
// "on ready" callback, we need to clear it on destruction of this class.
|
||||
// This clearing is not needed for other rclcpp entities like pub/subs, since
|
||||
// they do own the underlying rmw entities, which are destroyed
|
||||
// on their rclcpp destructors, thus no risk of dangling pointers.
|
||||
clear_on_ready_callback();
|
||||
}
|
||||
|
||||
/// Take data so that the callback cannot be scheduled again
|
||||
std::shared_ptr<void>
|
||||
take_data() override
|
||||
|
||||
@@ -206,14 +206,6 @@ public:
|
||||
const std::vector<std::string> unknown_ros_args;
|
||||
};
|
||||
|
||||
/// Thrown when an unknown type is passed
|
||||
class UnknownTypeError : public std::runtime_error
|
||||
{
|
||||
public:
|
||||
explicit UnknownTypeError(const std::string & type)
|
||||
: std::runtime_error("Unknown type: " + type) {}
|
||||
};
|
||||
|
||||
/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
|
||||
class InvalidEventError : public std::runtime_error
|
||||
{
|
||||
@@ -230,14 +222,6 @@ public:
|
||||
: std::runtime_error("event already registered") {}
|
||||
};
|
||||
|
||||
/// Thrown when a callback group is missing from the node, when it wants to utilize the group.
|
||||
class MissingGroupNodeException : public std::runtime_error
|
||||
{
|
||||
public:
|
||||
explicit MissingGroupNodeException(const std::string & obj_type)
|
||||
: std::runtime_error("cannot create: " + obj_type + " , callback group not in node") {}
|
||||
};
|
||||
|
||||
/// Thrown if passed parameters are inconsistent or invalid
|
||||
class InvalidParametersException : public std::runtime_error
|
||||
{
|
||||
|
||||
@@ -51,7 +51,6 @@ typedef std::map<rclcpp::CallbackGroup::WeakPtr,
|
||||
|
||||
// Forward declaration is used in convenience method signature.
|
||||
class Node;
|
||||
class ExecutorImplementation;
|
||||
|
||||
/// Coordinate the order and timing of available communication tasks.
|
||||
/**
|
||||
@@ -282,43 +281,21 @@ public:
|
||||
void
|
||||
spin_node_some(std::shared_ptr<rclcpp::Node> node);
|
||||
|
||||
/// Collect work once and execute all available work, optionally within a max duration.
|
||||
/// Collect work once and execute all available work, optionally within a duration.
|
||||
/**
|
||||
* This function can be overridden.
|
||||
* The default implementation is suitable for a single-threaded model of execution.
|
||||
* Adding subscriptions, timers, services, etc. with blocking or long running
|
||||
* callbacks may cause the function exceed the max_duration significantly.
|
||||
*
|
||||
* If there is no work to be done when this called, it will return immediately
|
||||
* because the collecting of available work is non-blocking.
|
||||
* Before each piece of ready work is executed this function checks if the
|
||||
* max_duration has been exceeded, and if it has it returns without starting
|
||||
* the execution of the next piece of work.
|
||||
*
|
||||
* If a max_duration of 0 is given, then all of the collected work will be
|
||||
* executed before the function returns.
|
||||
* 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));
|
||||
|
||||
/// Add a node, complete all immediately available work exhaustively, and remove the node.
|
||||
/**
|
||||
* \param[in] node Shared pointer to the node to add.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_node_all(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
|
||||
std::chrono::nanoseconds max_duration);
|
||||
|
||||
/// Convenience function which takes Node and forwards NodeBaseInterface.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_node_all(std::shared_ptr<rclcpp::Node> node, std::chrono::nanoseconds max_duration);
|
||||
|
||||
/// Collect and execute work repeatedly within a duration or until no more work is available.
|
||||
/**
|
||||
* This function can be overridden. The default implementation is suitable for a
|
||||
@@ -660,9 +637,8 @@ protected:
|
||||
std::atomic_bool spinning;
|
||||
|
||||
/// Guard condition for signaling the rmw layer to wake up for special events.
|
||||
std::shared_ptr<rclcpp::GuardCondition> interrupt_guard_condition_;
|
||||
rclcpp::GuardCondition interrupt_guard_condition_;
|
||||
|
||||
/// Guard condition for signaling the rmw layer to wake up for system shutdown.
|
||||
std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
|
||||
|
||||
/// Wait set for managing entities that the rmw layer waits on.
|
||||
@@ -720,9 +696,6 @@ protected:
|
||||
|
||||
/// shutdown callback handle registered to Context
|
||||
rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
|
||||
|
||||
/// Pointer to implementation
|
||||
std::unique_ptr<ExecutorImplementation> impl_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -21,7 +21,6 @@
|
||||
#include "rclcpp/executors/multi_threaded_executor.hpp"
|
||||
#include "rclcpp/executors/single_threaded_executor.hpp"
|
||||
#include "rclcpp/executors/static_single_threaded_executor.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
#include "rclcpp/utilities.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
@@ -29,18 +28,6 @@
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
/// Create a default single-threaded executor and execute all available work exhaustively.
|
||||
/** \param[in] node_ptr Shared pointer to the node to spin. */
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_all(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
std::chrono::nanoseconds max_duration);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration);
|
||||
|
||||
/// 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
|
||||
|
||||
@@ -1,213 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
|
||||
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
|
||||
|
||||
#include <deque>
|
||||
#include <functional>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <rclcpp/any_executable.hpp>
|
||||
#include <rclcpp/node_interfaces/node_base.hpp>
|
||||
#include <rclcpp/callback_group.hpp>
|
||||
#include <rclcpp/executors/executor_notify_waitable.hpp>
|
||||
#include <rclcpp/visibility_control.hpp>
|
||||
#include <rclcpp/wait_result.hpp>
|
||||
#include <rclcpp/wait_set.hpp>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Structure to represent a single entity's entry in a collection
|
||||
template<typename EntityValueType>
|
||||
struct CollectionEntry
|
||||
{
|
||||
/// Weak pointer to entity type
|
||||
using EntityWeakPtr = typename EntityValueType::WeakPtr;
|
||||
/// Shared pointer to entity type
|
||||
using EntitySharedPtr = typename EntityValueType::SharedPtr;
|
||||
|
||||
/// The entity
|
||||
EntityWeakPtr entity;
|
||||
|
||||
/// If relevant, the entity's corresponding callback_group
|
||||
rclcpp::CallbackGroup::WeakPtr callback_group;
|
||||
};
|
||||
|
||||
/// Update a collection based on another collection
|
||||
/*
|
||||
* Iterates update_from and update_to to see which entities have been added/removed between
|
||||
* the two collections.
|
||||
*
|
||||
* For each new entry (in update_from, but not in update_to),
|
||||
* add the entity and fire the on_added callback
|
||||
* For each removed entry (in update_to, but not in update_from),
|
||||
* remove the entity and fire the on_removed callback.
|
||||
*
|
||||
* \param[in] update_from The collection representing the next iteration's state
|
||||
* \param[inout] update_to The collection representing the current iteration's state
|
||||
* \param[in] on_added Callback fired when a new entity is detected
|
||||
* \param[in] on_removed Callback fired when an entity is removed
|
||||
*/
|
||||
template<typename CollectionType>
|
||||
void update_entities(
|
||||
const CollectionType & update_from,
|
||||
CollectionType & update_to,
|
||||
std::function<void(const typename CollectionType::EntitySharedPtr &)> on_added,
|
||||
std::function<void(const typename CollectionType::EntitySharedPtr &)> on_removed
|
||||
)
|
||||
{
|
||||
for (auto it = update_to.begin(); it != update_to.end(); ) {
|
||||
if (update_from.count(it->first) == 0) {
|
||||
auto entity = it->second.entity.lock();
|
||||
if (entity) {
|
||||
on_removed(entity);
|
||||
}
|
||||
it = update_to.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
for (auto it = update_from.begin(); it != update_from.end(); ++it) {
|
||||
if (update_to.count(it->first) == 0) {
|
||||
auto entity = it->second.entity.lock();
|
||||
if (entity) {
|
||||
on_added(entity);
|
||||
}
|
||||
update_to.insert(*it);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// A collection of entities, indexed by their corresponding handles
|
||||
template<typename EntityKeyType, typename EntityValueType>
|
||||
class EntityCollection
|
||||
: public std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>
|
||||
{
|
||||
public:
|
||||
/// Key type of the map
|
||||
using Key = const EntityKeyType *;
|
||||
|
||||
/// Weak pointer to entity type
|
||||
using EntityWeakPtr = typename EntityValueType::WeakPtr;
|
||||
|
||||
/// Shared pointer to entity type
|
||||
using EntitySharedPtr = typename EntityValueType::SharedPtr;
|
||||
|
||||
/// Update this collection based on the contents of another collection
|
||||
/**
|
||||
* Update the internal state of this collection, firing callbacks when entities have been
|
||||
* added or removed.
|
||||
*
|
||||
* \param[in] other Collection to compare to
|
||||
* \param[in] on_added Callback for when entities have been added
|
||||
* \param[in] on_removed Callback for when entities have been removed
|
||||
*/
|
||||
void update(
|
||||
const EntityCollection<EntityKeyType, EntityValueType> & other,
|
||||
std::function<void(const EntitySharedPtr &)> on_added,
|
||||
std::function<void(const EntitySharedPtr &)> on_removed)
|
||||
{
|
||||
update_entities(other, *this, on_added, on_removed);
|
||||
}
|
||||
};
|
||||
|
||||
/// Represent the total set of entities for a single executor
|
||||
/**
|
||||
* This allows the entities to be stored from ExecutorEntitiesCollector.
|
||||
* The structure also makes in convenient to re-evaluate when entities have been added or removed.
|
||||
*/
|
||||
struct ExecutorEntitiesCollection
|
||||
{
|
||||
/// Collection type for timer entities
|
||||
using TimerCollection = EntityCollection<rcl_timer_t, rclcpp::TimerBase>;
|
||||
|
||||
/// Collection type for subscription entities
|
||||
using SubscriptionCollection = EntityCollection<rcl_subscription_t, rclcpp::SubscriptionBase>;
|
||||
|
||||
/// Collection type for client entities
|
||||
using ClientCollection = EntityCollection<rcl_client_t, rclcpp::ClientBase>;
|
||||
|
||||
/// Collection type for service entities
|
||||
using ServiceCollection = EntityCollection<rcl_service_t, rclcpp::ServiceBase>;
|
||||
|
||||
/// Collection type for waitable entities
|
||||
using WaitableCollection = EntityCollection<rclcpp::Waitable, rclcpp::Waitable>;
|
||||
|
||||
/// Collection type for guard condition entities
|
||||
using GuardConditionCollection = EntityCollection<rcl_guard_condition_t, rclcpp::GuardCondition>;
|
||||
|
||||
/// Collection of timers currently in use by the executor.
|
||||
TimerCollection timers;
|
||||
|
||||
/// Collection of subscriptions currently in use by the executor.
|
||||
SubscriptionCollection subscriptions;
|
||||
|
||||
/// Collection of clients currently in use by the executor.
|
||||
ClientCollection clients;
|
||||
|
||||
/// Collection of services currently in use by the executor.
|
||||
ServiceCollection services;
|
||||
|
||||
/// Collection of guard conditions currently in use by the executor.
|
||||
GuardConditionCollection guard_conditions;
|
||||
|
||||
/// Collection of waitables currently in use by the executor.
|
||||
WaitableCollection waitables;
|
||||
|
||||
/// Check if the entities collection is empty
|
||||
/**
|
||||
* \return true if all member collections are empty, false otherwise
|
||||
*/
|
||||
bool empty() const;
|
||||
|
||||
/// Clear the entities collection
|
||||
void clear();
|
||||
};
|
||||
|
||||
/// Build an entities collection from callback groups
|
||||
/**
|
||||
* Iterates a list of callback groups and adds entities from each valid group
|
||||
*
|
||||
* \param[in] callback_groups List of callback groups to check for entities
|
||||
* \param[inout] colletion Entities collection to populate with found entities
|
||||
*/
|
||||
void
|
||||
build_entities_collection(
|
||||
const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
|
||||
ExecutorEntitiesCollection & collection);
|
||||
|
||||
/// Build a queue of executables ready to be executed
|
||||
/**
|
||||
* Iterates a list of entities and adds them to a queue if they are ready.
|
||||
*
|
||||
* \param[in] collection Collection of entities corresponding to the current wait set.
|
||||
* \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection.
|
||||
* \param[inout] queue of executables to append new ready executables to
|
||||
* \return number of new ready executables
|
||||
*/
|
||||
size_t
|
||||
ready_executables(
|
||||
const ExecutorEntitiesCollection & collection,
|
||||
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
|
||||
std::deque<rclcpp::AnyExecutable> & executables
|
||||
);
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
|
||||
@@ -1,270 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
|
||||
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
#include "rcpputils/thread_safety_annotations.hpp"
|
||||
|
||||
#include <rclcpp/any_executable.hpp>
|
||||
#include <rclcpp/node_interfaces/node_base.hpp>
|
||||
#include <rclcpp/callback_group.hpp>
|
||||
#include <rclcpp/executors/executor_notify_waitable.hpp>
|
||||
#include <rclcpp/visibility_control.hpp>
|
||||
#include <rclcpp/wait_set.hpp>
|
||||
#include <rclcpp/wait_result.hpp>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Class to monitor a set of nodes and callback groups for changes in entity membership
|
||||
/**
|
||||
* This is to be used with an executor to track the membership of various nodes, groups,
|
||||
* and entities (timers, subscriptions, clients, services, etc) and report status to the
|
||||
* executor.
|
||||
*
|
||||
* In general, users will add either nodes or callback groups to an executor.
|
||||
* Each node may have callback groups that are automatically associated with executors,
|
||||
* or callback groups that must be manually associated with an executor.
|
||||
*
|
||||
* This object tracks both types of callback groups as well as nodes that have been
|
||||
* previously added to the executor.
|
||||
* When a new callback group is added/removed or new entities are added/removed, the
|
||||
* corresponding node or callback group will signal this to the executor so that the
|
||||
* entity collection may be rebuilt according to that executor's implementation.
|
||||
*
|
||||
*/
|
||||
class ExecutorEntitiesCollector
|
||||
{
|
||||
public:
|
||||
/// Constructor
|
||||
/**
|
||||
* \param[in] notify_waitable Waitable that is used to signal to the executor
|
||||
* when nodes or callback groups have been added or removed.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit ExecutorEntitiesCollector(
|
||||
std::shared_ptr<ExecutorNotifyWaitable> notify_waitable);
|
||||
|
||||
/// Destructor
|
||||
RCLCPP_PUBLIC
|
||||
~ExecutorEntitiesCollector();
|
||||
|
||||
/// Indicate if the entities collector has pending additions or removals.
|
||||
/**
|
||||
* \return true if there are pending additions or removals
|
||||
*/
|
||||
bool has_pending() const;
|
||||
|
||||
/// Add a node to the entity collector
|
||||
/**
|
||||
* \param[in] node_ptr a shared pointer that points to a node base interface
|
||||
* \throw std::runtime_error if the node is associated with an executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
|
||||
|
||||
/// Remove a node from the entity collector
|
||||
/**
|
||||
* \param[in] node_ptr a shared pointer that points to a node base interface
|
||||
* \throw std::runtime_error if the node is associated with an executor
|
||||
* \throw std::runtime_error if the node is associated with this executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
|
||||
|
||||
/// Add a callback group to the entity collector
|
||||
/**
|
||||
* \param[in] group_ptr a shared pointer that points to a callback group
|
||||
* \throw std::runtime_error if the callback_group is associated with an executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
|
||||
|
||||
/// Remove a callback group from the entity collector
|
||||
/**
|
||||
* \param[in] group_ptr a shared pointer that points to a callback group
|
||||
* \throw std::runtime_error if the callback_group is not associated with an executor
|
||||
* \throw std::runtime_error if the callback_group is not associated with this executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
|
||||
|
||||
/// Get all callback groups known to this entity collector
|
||||
/**
|
||||
* This will include manually added and automatically added (node associated) groups
|
||||
* \return vector of all callback groups
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_all_callback_groups() const;
|
||||
|
||||
/// Get manually-added callback groups known to this entity collector
|
||||
/**
|
||||
* This will include callback groups that have been added via add_callback_group
|
||||
* \return vector of manually-added callback groups
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_manually_added_callback_groups() const;
|
||||
|
||||
/// Get automatically-added callback groups known to this entity collector
|
||||
/**
|
||||
* This will include callback groups that are associated with nodes added via add_node
|
||||
* \return vector of automatically-added callback groups
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_automatically_added_callback_groups() const;
|
||||
|
||||
/// Update the underlying collections
|
||||
/**
|
||||
* This will prune nodes and callback groups that are no longer valid as well
|
||||
* as add new callback groups from any associated nodes.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
update_collections();
|
||||
|
||||
protected:
|
||||
using NodeCollection = std::set<
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
|
||||
std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
|
||||
|
||||
using CallbackGroupCollection = std::set<
|
||||
rclcpp::CallbackGroup::WeakPtr,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
|
||||
|
||||
using WeakNodesToGuardConditionsMap = std::map<
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
|
||||
rclcpp::GuardCondition::WeakPtr,
|
||||
std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
|
||||
|
||||
using WeakGroupsToGuardConditionsMap = std::map<
|
||||
rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::GuardCondition::WeakPtr,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
|
||||
|
||||
/// Implementation of removing a node from the collector.
|
||||
/**
|
||||
* This will disassociate the node from the collector and remove any
|
||||
* automatically-added callback groups
|
||||
*
|
||||
* This takes and returns an iterator so it may be used as:
|
||||
*
|
||||
* it = remove_weak_node(it);
|
||||
*
|
||||
* \param[in] weak_node iterator to the weak node to be removed
|
||||
* \return Valid updated iterator in the same collection
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
NodeCollection::iterator
|
||||
remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Implementation of removing a callback group from the collector.
|
||||
/**
|
||||
* This will disassociate the callback group from the collector
|
||||
*
|
||||
* This takes and returns an iterator so it may be used as:
|
||||
*
|
||||
* it = remove_weak_callback_group(it);
|
||||
*
|
||||
* \param[in] weak_group_it iterator to the weak group to be removed
|
||||
* \param[in] collection the collection to remove the group from
|
||||
* (manually or automatically added)
|
||||
* \return Valid updated iterator in the same collection
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
CallbackGroupCollection::iterator
|
||||
remove_weak_callback_group(
|
||||
CallbackGroupCollection::iterator weak_group_it,
|
||||
CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Implementation of adding a callback group
|
||||
/**
|
||||
* \param[in] group_ptr the group to add
|
||||
* \param[in] collection the collection to add the group to
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_callback_group_to_collection(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Iterate over queued added/remove nodes and callback_groups
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
process_queues() RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Check a collection of nodes and add any new callback_groups that
|
||||
/// are set to be automatically associated via the node.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_automatically_associated_callback_groups(
|
||||
const NodeCollection & nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Check all nodes and group for expired weak pointers and remove them.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// mutex to protect collections and pending queues
|
||||
mutable std::mutex mutex_;
|
||||
|
||||
/// Callback groups that were added via `add_callback_group`
|
||||
CallbackGroupCollection manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Callback groups that were added by their association with added nodes
|
||||
CallbackGroupCollection automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// nodes that are associated with the executor
|
||||
NodeCollection weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Track guard conditions associated with added nodes
|
||||
WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Track guard conditions associated with added callback groups
|
||||
WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// nodes that have been added since the last update.
|
||||
NodeCollection pending_added_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// nodes that have been removed since the last update.
|
||||
NodeCollection pending_removed_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// callback groups that have been added since the last update.
|
||||
CallbackGroupCollection pending_manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// callback groups that have been removed since the last update.
|
||||
CallbackGroupCollection pending_manually_removed_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Waitable to add guard conditions to
|
||||
std::shared_ptr<ExecutorNotifyWaitable> notify_waitable_;
|
||||
};
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
//
|
||||
#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
|
||||
@@ -1,158 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
|
||||
#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
|
||||
#include "rclcpp/guard_condition.hpp"
|
||||
#include "rclcpp/waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Maintain a collection of guard conditions from associated nodes and callback groups
|
||||
/// to signal to the executor when associated entities have changed.
|
||||
class ExecutorNotifyWaitable : public rclcpp::Waitable
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable)
|
||||
|
||||
// Constructor
|
||||
/**
|
||||
* \param[in] on_execute_callback Callback to execute when one of the conditions
|
||||
* of this waitable has signaled the wait_set.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback = {});
|
||||
|
||||
// Destructor
|
||||
RCLCPP_PUBLIC
|
||||
~ExecutorNotifyWaitable() override = default;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other);
|
||||
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other);
|
||||
|
||||
/// Add conditions to the wait set
|
||||
/**
|
||||
* \param[inout] wait_set structure that conditions will be added to
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_to_wait_set(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Check conditions against the wait set
|
||||
/**
|
||||
* \param[inout] wait_set structure that internal elements will be checked against.
|
||||
* \return true if this waitable is ready to be executed, false otherwise.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_ready(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Perform work associated with the waitable.
|
||||
/**
|
||||
* This will call the callback provided in the constructor.
|
||||
* \param[in] data Data to be use for the execute, if available, else nullptr.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
execute(std::shared_ptr<void> & data) override;
|
||||
|
||||
/// Retrieve data to be used in the next execute call.
|
||||
/**
|
||||
* \return If available, data to be used, otherwise nullptr
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<void>
|
||||
take_data() override;
|
||||
|
||||
/// Take the data from an entity ID so that it can be consumed with `execute`.
|
||||
/**
|
||||
* \param[in] id ID of the entity to take data from.
|
||||
* \return If available, data to be used, otherwise nullptr
|
||||
* \sa rclcpp::Waitable::take_data_by_entity_id
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<void>
|
||||
take_data_by_entity_id(size_t id) override;
|
||||
|
||||
/// Set a callback to be called whenever the waitable becomes ready.
|
||||
/**
|
||||
* \param[in] callback callback to set
|
||||
* \sa rclcpp::Waitable::set_on_ready_callback
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_on_ready_callback(std::function<void(size_t, int)> callback) override;
|
||||
|
||||
/// Add a guard condition to be waited on.
|
||||
/**
|
||||
* \param[in] guard_condition The guard condition to add.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition);
|
||||
|
||||
/// Unset any callback registered via set_on_ready_callback.
|
||||
/**
|
||||
* \sa rclcpp::Waitable::clear_on_ready_callback
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_on_ready_callback() override;
|
||||
|
||||
/// Remove a guard condition from being waited on.
|
||||
/**
|
||||
* \param[in] weak_guard_condition The guard condition to remove.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition);
|
||||
|
||||
/// Get the number of ready guard_conditions
|
||||
/**
|
||||
* \return The number of guard_conditions associated with the Waitable.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_guard_conditions() override;
|
||||
|
||||
private:
|
||||
/// Callback to run when waitable executes
|
||||
std::function<void(void)> execute_callback_;
|
||||
|
||||
std::mutex guard_condition_mutex_;
|
||||
|
||||
std::function<void(size_t)> on_ready_callback_;
|
||||
|
||||
/// The collection of guard conditions to be waited on.
|
||||
std::set<rclcpp::GuardCondition::WeakPtr,
|
||||
std::owner_less<rclcpp::GuardCondition::WeakPtr>> notify_guard_conditions_;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
|
||||
@@ -15,8 +15,6 @@
|
||||
#ifndef RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
@@ -33,11 +31,8 @@ public:
|
||||
virtual BufferT dequeue() = 0;
|
||||
virtual void enqueue(BufferT request) = 0;
|
||||
|
||||
virtual std::vector<BufferT> get_all_data() = 0;
|
||||
|
||||
virtual void clear() = 0;
|
||||
virtual bool has_data() const = 0;
|
||||
virtual size_t available_capacity() const = 0;
|
||||
};
|
||||
|
||||
} // namespace buffers
|
||||
|
||||
@@ -19,13 +19,11 @@
|
||||
#include <stdexcept>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/allocator/allocator_common.hpp"
|
||||
#include "rclcpp/allocator/allocator_deleter.hpp"
|
||||
#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "tracetools/tracetools.h"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
@@ -45,7 +43,6 @@ public:
|
||||
|
||||
virtual bool has_data() const = 0;
|
||||
virtual bool use_take_shared_method() const = 0;
|
||||
virtual size_t available_capacity() const = 0;
|
||||
};
|
||||
|
||||
template<
|
||||
@@ -67,9 +64,6 @@ public:
|
||||
|
||||
virtual MessageSharedPtr consume_shared() = 0;
|
||||
virtual MessageUniquePtr consume_unique() = 0;
|
||||
|
||||
virtual std::vector<MessageSharedPtr> get_all_data_shared() = 0;
|
||||
virtual std::vector<MessageUniquePtr> get_all_data_unique() = 0;
|
||||
};
|
||||
|
||||
template<
|
||||
@@ -100,10 +94,6 @@ public:
|
||||
|
||||
buffer_ = std::move(buffer_impl);
|
||||
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_buffer_to_ipb,
|
||||
static_cast<const void *>(buffer_.get()),
|
||||
static_cast<const void *>(this));
|
||||
if (!allocator) {
|
||||
message_allocator_ = std::make_shared<MessageAlloc>();
|
||||
} else {
|
||||
@@ -133,16 +123,6 @@ public:
|
||||
return consume_unique_impl<BufferT>();
|
||||
}
|
||||
|
||||
std::vector<MessageSharedPtr> get_all_data_shared() override
|
||||
{
|
||||
return get_all_data_shared_impl();
|
||||
}
|
||||
|
||||
std::vector<MessageUniquePtr> get_all_data_unique() override
|
||||
{
|
||||
return get_all_data_unique_impl();
|
||||
}
|
||||
|
||||
bool has_data() const override
|
||||
{
|
||||
return buffer_->has_data();
|
||||
@@ -158,11 +138,6 @@ public:
|
||||
return std::is_same<BufferT, MessageSharedPtr>::value;
|
||||
}
|
||||
|
||||
size_t available_capacity() const override
|
||||
{
|
||||
return buffer_->available_capacity();
|
||||
}
|
||||
|
||||
private:
|
||||
std::unique_ptr<BufferImplementationBase<BufferT>> buffer_;
|
||||
|
||||
@@ -257,71 +232,6 @@ private:
|
||||
{
|
||||
return buffer_->dequeue();
|
||||
}
|
||||
|
||||
// MessageSharedPtr to MessageSharedPtr
|
||||
template<typename T = BufferT>
|
||||
typename std::enable_if<
|
||||
std::is_same<T, MessageSharedPtr>::value,
|
||||
std::vector<MessageSharedPtr>
|
||||
>::type
|
||||
get_all_data_shared_impl()
|
||||
{
|
||||
return buffer_->get_all_data();
|
||||
}
|
||||
|
||||
// MessageUniquePtr to MessageSharedPtr
|
||||
template<typename T = BufferT>
|
||||
typename std::enable_if<
|
||||
std::is_same<T, MessageUniquePtr>::value,
|
||||
std::vector<MessageSharedPtr>
|
||||
>::type
|
||||
get_all_data_shared_impl()
|
||||
{
|
||||
std::vector<MessageSharedPtr> result;
|
||||
auto uni_ptr_vec = buffer_->get_all_data();
|
||||
result.reserve(uni_ptr_vec.size());
|
||||
for (MessageUniquePtr & uni_ptr : uni_ptr_vec) {
|
||||
result.emplace_back(std::move(uni_ptr));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// MessageSharedPtr to MessageUniquePtr
|
||||
template<typename T = BufferT>
|
||||
typename std::enable_if<
|
||||
std::is_same<T, MessageSharedPtr>::value,
|
||||
std::vector<MessageUniquePtr>
|
||||
>::type
|
||||
get_all_data_unique_impl()
|
||||
{
|
||||
std::vector<MessageUniquePtr> result;
|
||||
auto shared_ptr_vec = buffer_->get_all_data();
|
||||
result.reserve(shared_ptr_vec.size());
|
||||
for (MessageSharedPtr shared_msg : shared_ptr_vec) {
|
||||
MessageUniquePtr unique_msg;
|
||||
MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(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);
|
||||
}
|
||||
result.push_back(std::move(unique_msg));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// MessageUniquePtr to MessageUniquePtr
|
||||
template<typename T = BufferT>
|
||||
typename std::enable_if<
|
||||
std::is_same<T, MessageUniquePtr>::value,
|
||||
std::vector<MessageUniquePtr>
|
||||
>::type
|
||||
get_all_data_unique_impl()
|
||||
{
|
||||
return buffer_->get_all_data();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace buffers
|
||||
|
||||
@@ -15,7 +15,6 @@
|
||||
#ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
@@ -26,7 +25,6 @@
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
#include "tracetools/tracetools.h"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
@@ -53,10 +51,6 @@ public:
|
||||
if (capacity == 0) {
|
||||
throw std::invalid_argument("capacity must be a positive, non-zero value");
|
||||
}
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_construct_ring_buffer,
|
||||
static_cast<const void *>(this),
|
||||
capacity_);
|
||||
}
|
||||
|
||||
virtual ~RingBufferImplementation() {}
|
||||
@@ -67,18 +61,12 @@ public:
|
||||
*
|
||||
* \param request the element to be stored in the ring buffer
|
||||
*/
|
||||
void enqueue(BufferT request) override
|
||||
void enqueue(BufferT request)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
write_index_ = next_(write_index_);
|
||||
ring_buffer_[write_index_] = std::move(request);
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_ring_buffer_enqueue,
|
||||
static_cast<const void *>(this),
|
||||
write_index_,
|
||||
size_ + 1,
|
||||
is_full_());
|
||||
|
||||
if (is_full_()) {
|
||||
read_index_ = next_(read_index_);
|
||||
@@ -93,7 +81,7 @@ public:
|
||||
*
|
||||
* \return the element that is being removed from the ring buffer
|
||||
*/
|
||||
BufferT dequeue() override
|
||||
BufferT dequeue()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
@@ -102,11 +90,6 @@ public:
|
||||
}
|
||||
|
||||
auto request = std::move(ring_buffer_[read_index_]);
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_ring_buffer_dequeue,
|
||||
static_cast<const void *>(this),
|
||||
read_index_,
|
||||
size_ - 1);
|
||||
read_index_ = next_(read_index_);
|
||||
|
||||
size_--;
|
||||
@@ -114,17 +97,6 @@ public:
|
||||
return request;
|
||||
}
|
||||
|
||||
/// Get all the elements from the ring buffer
|
||||
/**
|
||||
* This member function is thread-safe.
|
||||
*
|
||||
* \return a vector containing all the elements from the ring buffer
|
||||
*/
|
||||
std::vector<BufferT> get_all_data() override
|
||||
{
|
||||
return get_all_data_impl();
|
||||
}
|
||||
|
||||
/// Get the next index value for the ring buffer
|
||||
/**
|
||||
* This member function is thread-safe.
|
||||
@@ -144,7 +116,7 @@ public:
|
||||
*
|
||||
* \return `true` if there is data and `false` otherwise
|
||||
*/
|
||||
inline bool has_data() const override
|
||||
inline bool has_data() const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return has_data_();
|
||||
@@ -163,22 +135,7 @@ public:
|
||||
return is_full_();
|
||||
}
|
||||
|
||||
/// Get the remaining capacity to store messages
|
||||
/**
|
||||
* This member function is thread-safe.
|
||||
*
|
||||
* \return the number of free capacity for new messages
|
||||
*/
|
||||
size_t available_capacity() const override
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return available_capacity_();
|
||||
}
|
||||
|
||||
void clear() override
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
|
||||
}
|
||||
void clear() {}
|
||||
|
||||
private:
|
||||
/// Get the next index value for the ring buffer
|
||||
@@ -216,82 +173,6 @@ private:
|
||||
return size_ == capacity_;
|
||||
}
|
||||
|
||||
/// Get the remaining capacity to store messages
|
||||
/**
|
||||
* This member function is not thread-safe.
|
||||
*
|
||||
* \return the number of free capacity for new messages
|
||||
*/
|
||||
inline size_t available_capacity_() const
|
||||
{
|
||||
return capacity_ - size_;
|
||||
}
|
||||
|
||||
/// Traits for checking if a type is std::unique_ptr
|
||||
template<typename ...>
|
||||
struct is_std_unique_ptr final : std::false_type {};
|
||||
template<class T, typename ... Args>
|
||||
struct is_std_unique_ptr<std::unique_ptr<T, Args...>> final : std::true_type
|
||||
{
|
||||
typedef T Ptr_type;
|
||||
};
|
||||
|
||||
/// Get all the elements from the ring buffer
|
||||
/**
|
||||
* This member function is thread-safe.
|
||||
* Two versions for the implementation of the function.
|
||||
* One for buffer containing unique_ptr and the other for other types
|
||||
*
|
||||
* \return a vector containing all the elements from the ring buffer
|
||||
*/
|
||||
template<typename T = BufferT, std::enable_if_t<is_std_unique_ptr<T>::value &&
|
||||
std::is_copy_constructible<
|
||||
typename is_std_unique_ptr<T>::Ptr_type
|
||||
>::value,
|
||||
void> * = nullptr>
|
||||
std::vector<BufferT> get_all_data_impl()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
std::vector<BufferT> result_vtr;
|
||||
result_vtr.reserve(size_);
|
||||
for (size_t id = 0; id < size_; ++id) {
|
||||
result_vtr.emplace_back(
|
||||
new typename is_std_unique_ptr<T>::Ptr_type(
|
||||
*(ring_buffer_[(read_index_ + id) % capacity_])));
|
||||
}
|
||||
return result_vtr;
|
||||
}
|
||||
|
||||
template<typename T = BufferT, std::enable_if_t<
|
||||
std::is_copy_constructible<T>::value, void> * = nullptr>
|
||||
std::vector<BufferT> get_all_data_impl()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
std::vector<BufferT> result_vtr;
|
||||
result_vtr.reserve(size_);
|
||||
for (size_t id = 0; id < size_; ++id) {
|
||||
result_vtr.emplace_back(ring_buffer_[(read_index_ + id) % capacity_]);
|
||||
}
|
||||
return result_vtr;
|
||||
}
|
||||
|
||||
template<typename T = BufferT, std::enable_if_t<!is_std_unique_ptr<T>::value &&
|
||||
!std::is_copy_constructible<T>::value, void> * = nullptr>
|
||||
std::vector<BufferT> get_all_data_impl()
|
||||
{
|
||||
throw std::logic_error("Underlined type results in invalid get_all_data_impl()");
|
||||
return {};
|
||||
}
|
||||
|
||||
template<typename T = BufferT, std::enable_if_t<is_std_unique_ptr<T>::value &&
|
||||
!std::is_copy_constructible<typename is_std_unique_ptr<T>::Ptr_type>::value,
|
||||
void> * = nullptr>
|
||||
std::vector<BufferT> get_all_data_impl()
|
||||
{
|
||||
throw std::logic_error("Underlined type in unique_ptr results in invalid get_all_data_impl()");
|
||||
return {};
|
||||
}
|
||||
|
||||
size_t capacity_;
|
||||
|
||||
std::vector<BufferT> ring_buffer_;
|
||||
|
||||
@@ -1,294 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/executor.hpp"
|
||||
#include "rclcpp/executors/executor_entities_collection.hpp"
|
||||
#include "rclcpp/executors/executor_entities_collector.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp"
|
||||
#include "rclcpp/experimental/timers_manager.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Events executor implementation
|
||||
/**
|
||||
* This executor uses an events queue and a timers manager to execute entities from its
|
||||
* associated nodes and callback groups.
|
||||
* ROS 2 entities allow to set callback functions that are invoked when the entity is triggered
|
||||
* or has work to do. The events-executor sets these callbacks such that they push an
|
||||
* event into its queue.
|
||||
*
|
||||
* This executor tries to reduce as much as possible the amount of maintenance operations.
|
||||
* This allows to use customized `EventsQueue` classes to achieve different goals such
|
||||
* as very low CPU usage, bounded memory requirement, determinism, etc.
|
||||
*
|
||||
* The executor uses a weak ownership model and it locks entities only while executing
|
||||
* their related events.
|
||||
*
|
||||
* To run this executor:
|
||||
* rclcpp::experimental::executors::EventsExecutor executor;
|
||||
* executor.add_node(node);
|
||||
* executor.spin();
|
||||
* executor.remove_node(node);
|
||||
*/
|
||||
class EventsExecutor : public rclcpp::Executor
|
||||
{
|
||||
friend class EventsExecutorEntitiesCollector;
|
||||
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
|
||||
|
||||
/// Default constructor. See the default constructor for Executor.
|
||||
/**
|
||||
* \param[in] events_queue The queue used to store events.
|
||||
* \param[in] execute_timers_separate_thread If true, timers are executed in a separate
|
||||
* thread. If false, timers are executed in the same thread as all other entities.
|
||||
* \param[in] options Options used to configure the executor.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit EventsExecutor(
|
||||
rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
|
||||
rclcpp::experimental::executors::SimpleEventsQueue>(),
|
||||
bool execute_timers_separate_thread = false,
|
||||
const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
|
||||
|
||||
/// Default destructor.
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~EventsExecutor();
|
||||
|
||||
/// Events executor implementation of spin.
|
||||
/**
|
||||
* This function will block until work comes in, execute it, and keep blocking.
|
||||
* It will only be interrupted by a CTRL-C (managed by the global signal handler).
|
||||
* \throws std::runtime_error when spin() called while already spinning
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin() override;
|
||||
|
||||
/// Events executor implementation of spin some
|
||||
/**
|
||||
* This non-blocking function will execute the timers and events
|
||||
* that were ready when this API was called, until timeout or no
|
||||
* more work available. New ready-timers/events arrived while
|
||||
* executing work, won't be taken into account here.
|
||||
*
|
||||
* Example:
|
||||
* while(condition) {
|
||||
* spin_some();
|
||||
* sleep(); // User should have some sync work or
|
||||
* // sleep to avoid a 100% CPU usage
|
||||
* }
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override;
|
||||
|
||||
/// Events executor implementation of spin all
|
||||
/**
|
||||
* This non-blocking function will execute timers and events
|
||||
* until timeout or no more work available. If new ready-timers/events
|
||||
* arrive while executing work available, they will be executed
|
||||
* as long as the timeout hasn't expired.
|
||||
*
|
||||
* Example:
|
||||
* while(condition) {
|
||||
* spin_all();
|
||||
* sleep(); // User should have some sync work or
|
||||
* // sleep to avoid a 100% CPU usage
|
||||
* }
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_all(std::chrono::nanoseconds max_duration) override;
|
||||
|
||||
/// Add a node to the executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::add_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Convenience function which takes Node and forwards NodeBaseInterface.
|
||||
/**
|
||||
* \sa rclcpp::EventsExecutor::add_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
|
||||
|
||||
/// Remove a node from the executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Convenience function which takes Node and forwards NodeBaseInterface.
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
|
||||
|
||||
/// Add a callback group to an executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::add_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Remove callback group from the executor
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_all_callback_groups()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_all_callback_groups() override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_manually_added_callback_groups()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_manually_added_callback_groups() override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_automatically_added_callback_groups_from_nodes() override;
|
||||
|
||||
protected:
|
||||
/// Internal implementation of spin_once
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_once_impl(std::chrono::nanoseconds timeout) override;
|
||||
|
||||
/// Internal implementation of spin_some
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(EventsExecutor)
|
||||
|
||||
/// Execute a provided executor event if its associated entities are available
|
||||
void
|
||||
execute_event(const ExecutorEvent & event);
|
||||
|
||||
/// Collect entities from callback groups and refresh the current collection with them
|
||||
void
|
||||
refresh_current_collection_from_callback_groups();
|
||||
|
||||
/// Refresh the current collection using the provided new_collection
|
||||
void
|
||||
refresh_current_collection(const rclcpp::executors::ExecutorEntitiesCollection & new_collection);
|
||||
|
||||
/// Create a listener callback function for the provided entity
|
||||
std::function<void(size_t)>
|
||||
create_entity_callback(void * entity_key, ExecutorEventType type);
|
||||
|
||||
/// Create a listener callback function for the provided waitable entity
|
||||
std::function<void(size_t, int)>
|
||||
create_waitable_callback(const rclcpp::Waitable * waitable_id);
|
||||
|
||||
/// Utility to add the notify waitable to an entities collection
|
||||
void
|
||||
add_notify_waitable_to_collection(
|
||||
rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection);
|
||||
|
||||
/// Searches for the provided entity_id in the collection and returns the entity if valid
|
||||
template<typename CollectionType>
|
||||
typename CollectionType::EntitySharedPtr
|
||||
retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection)
|
||||
{
|
||||
// Check if the entity_id is in the collection
|
||||
auto it = collection.find(entity_id);
|
||||
if (it == collection.end()) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Check if the entity associated with the entity_id is valid
|
||||
// and remove it from the collection if it isn't
|
||||
auto entity = it->second.entity.lock();
|
||||
if (!entity) {
|
||||
collection.erase(it);
|
||||
}
|
||||
|
||||
// Return the retrieved entity (this can be a nullptr if the entity was not valid)
|
||||
return entity;
|
||||
}
|
||||
|
||||
/// Queue where entities can push events
|
||||
rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
|
||||
|
||||
std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector_;
|
||||
std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
|
||||
|
||||
/// Mutex to protect the current_entities_collection_
|
||||
std::recursive_mutex collection_mutex_;
|
||||
std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollection> current_entities_collection_;
|
||||
|
||||
/// Flag used to reduce the number of unnecessary waitable events
|
||||
std::atomic<bool> notify_waitable_event_pushed_ {false};
|
||||
|
||||
/// Timers manager used to track and/or execute associated timers
|
||||
std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
|
||||
@@ -1,46 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
enum ExecutorEventType
|
||||
{
|
||||
CLIENT_EVENT,
|
||||
SUBSCRIPTION_EVENT,
|
||||
SERVICE_EVENT,
|
||||
TIMER_EVENT,
|
||||
WAITABLE_EVENT
|
||||
};
|
||||
|
||||
struct ExecutorEvent
|
||||
{
|
||||
const void * entity_key;
|
||||
int waitable_data;
|
||||
ExecutorEventType type;
|
||||
size_t num_events;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
|
||||
@@ -1,100 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
|
||||
|
||||
#include <queue>
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief This abstract class can be used to implement different types of queues
|
||||
* where `ExecutorEvent` can be stored.
|
||||
* The derived classes should choose which underlying container to use and
|
||||
* the strategy for pushing and popping events.
|
||||
* For example a queue implementation may be bounded or unbounded and have
|
||||
* different pruning strategies.
|
||||
* Implementations may or may not check the validity of events and decide how to handle
|
||||
* the situation where an event is not valid anymore (e.g. a subscription history cache overruns)
|
||||
*/
|
||||
class EventsQueue
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(EventsQueue)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
EventsQueue() = default;
|
||||
|
||||
/**
|
||||
* @brief Destruct the object.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~EventsQueue() = default;
|
||||
|
||||
/**
|
||||
* @brief push event into the queue
|
||||
* @param event The event to push into the queue
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
void
|
||||
enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) = 0;
|
||||
|
||||
/**
|
||||
* @brief Extracts an event from the queue, eventually waiting until timeout
|
||||
* if none is available.
|
||||
* @return true if event has been found, false if timeout
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
bool
|
||||
dequeue(
|
||||
rclcpp::experimental::executors::ExecutorEvent & event,
|
||||
std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) = 0;
|
||||
|
||||
/**
|
||||
* @brief Test whether queue is empty
|
||||
* @return true if the queue's size is 0, false otherwise.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
bool
|
||||
empty() const = 0;
|
||||
|
||||
/**
|
||||
* @brief Returns the number of elements in the queue.
|
||||
* @return the number of elements in the queue.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
size_t
|
||||
size() const = 0;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
|
||||
@@ -1,134 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
|
||||
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief This class implements an EventsQueue as a simple wrapper around a std::queue.
|
||||
* It does not perform any checks about the size of queue, which can grow
|
||||
* unbounded without being pruned.
|
||||
* The simplicity of this implementation makes it suitable for optimizing CPU usage.
|
||||
*/
|
||||
class SimpleEventsQueue : public EventsQueue
|
||||
{
|
||||
public:
|
||||
RCLCPP_PUBLIC
|
||||
~SimpleEventsQueue() override = default;
|
||||
|
||||
/**
|
||||
* @brief enqueue event into the queue
|
||||
* Thread safe
|
||||
* @param event The event to enqueue into the queue
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) override
|
||||
{
|
||||
rclcpp::experimental::executors::ExecutorEvent single_event = event;
|
||||
single_event.num_events = 1;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
for (size_t ev = 0; ev < event.num_events; ev++) {
|
||||
event_queue_.push(single_event);
|
||||
}
|
||||
}
|
||||
events_queue_cv_.notify_one();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief waits for an event until timeout, gets a single event
|
||||
* Thread safe
|
||||
* @return true if event, false if timeout
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
dequeue(
|
||||
rclcpp::experimental::executors::ExecutorEvent & event,
|
||||
std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) override
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
|
||||
// Initialize to true because it's only needed if we have a valid timeout
|
||||
bool has_data = true;
|
||||
if (timeout != std::chrono::nanoseconds::max()) {
|
||||
has_data =
|
||||
events_queue_cv_.wait_for(lock, timeout, [this]() {return !event_queue_.empty();});
|
||||
} else {
|
||||
events_queue_cv_.wait(lock, [this]() {return !event_queue_.empty();});
|
||||
}
|
||||
|
||||
if (has_data) {
|
||||
event = event_queue_.front();
|
||||
event_queue_.pop();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Test whether queue is empty
|
||||
* Thread safe
|
||||
* @return true if the queue's size is 0, false otherwise.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
empty() const override
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
return event_queue_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the number of elements in the queue.
|
||||
* Thread safe
|
||||
* @return the number of elements in the queue.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
size() const override
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
return event_queue_.size();
|
||||
}
|
||||
|
||||
private:
|
||||
// The underlying queue implementation
|
||||
std::queue<rclcpp::experimental::executors::ExecutorEvent> event_queue_;
|
||||
// Mutex to protect read/write access to the queue
|
||||
mutable std::mutex mutex_;
|
||||
// Variable used to notify when an event is added to the queue
|
||||
std::condition_variable events_queue_cv_;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
|
||||
@@ -28,7 +28,6 @@
|
||||
#include <typeinfo>
|
||||
|
||||
#include "rclcpp/allocator/allocator_deleter.hpp"
|
||||
#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
|
||||
#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp"
|
||||
#include "rclcpp/experimental/subscription_intra_process.hpp"
|
||||
#include "rclcpp/experimental/subscription_intra_process_base.hpp"
|
||||
@@ -113,40 +112,9 @@ public:
|
||||
* \param subscription the SubscriptionIntraProcess to register.
|
||||
* \return an unsigned 64-bit integer which is the subscription's unique id.
|
||||
*/
|
||||
template<
|
||||
typename ROSMessageType,
|
||||
typename Alloc = std::allocator<ROSMessageType>
|
||||
>
|
||||
RCLCPP_PUBLIC
|
||||
uint64_t
|
||||
add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription)
|
||||
{
|
||||
std::unique_lock<std::shared_timed_mutex> lock(mutex_);
|
||||
|
||||
uint64_t sub_id = IntraProcessManager::get_next_unique_id();
|
||||
|
||||
subscriptions_[sub_id] = subscription;
|
||||
|
||||
// adds the subscription id to all the matchable publishers
|
||||
for (auto & pair : publishers_) {
|
||||
auto publisher = pair.second.lock();
|
||||
if (!publisher) {
|
||||
continue;
|
||||
}
|
||||
if (can_communicate(publisher, subscription)) {
|
||||
uint64_t pub_id = pair.first;
|
||||
insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
|
||||
if (publisher->is_durability_transient_local() &&
|
||||
subscription->is_durability_transient_local())
|
||||
{
|
||||
do_transient_local_publish<ROSMessageType, Alloc>(
|
||||
pub_id, sub_id,
|
||||
subscription->use_take_shared_method());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return sub_id;
|
||||
}
|
||||
add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription);
|
||||
|
||||
/// Unregister a subscription using the subscription's unique id.
|
||||
/**
|
||||
@@ -163,21 +131,14 @@ public:
|
||||
* This method stores the publisher intra process object, together with
|
||||
* the information of its wrapped publisher (i.e. topic name and QoS).
|
||||
*
|
||||
* If the publisher's durability is transient local, its buffer pointer should
|
||||
* be passed and the method will store it as well.
|
||||
*
|
||||
* In addition this generates a unique intra process id for the publisher.
|
||||
*
|
||||
* \param publisher publisher to be registered with the manager.
|
||||
* \param buffer publisher's buffer to be stored if its duability is transient local.
|
||||
* \return an unsigned 64-bit integer which is the publisher's unique id.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
uint64_t
|
||||
add_publisher(
|
||||
rclcpp::PublisherBase::SharedPtr publisher,
|
||||
rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer =
|
||||
rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr());
|
||||
add_publisher(rclcpp::PublisherBase::SharedPtr publisher);
|
||||
|
||||
/// Unregister a publisher using the publisher's unique id.
|
||||
/**
|
||||
@@ -331,34 +292,6 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
template<
|
||||
typename MessageT,
|
||||
typename Alloc,
|
||||
typename Deleter,
|
||||
typename ROSMessageType>
|
||||
void
|
||||
add_shared_msg_to_buffer(
|
||||
std::shared_ptr<const MessageT> message,
|
||||
uint64_t subscription_id)
|
||||
{
|
||||
add_shared_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(message, {subscription_id});
|
||||
}
|
||||
|
||||
template<
|
||||
typename MessageT,
|
||||
typename Alloc,
|
||||
typename Deleter,
|
||||
typename ROSMessageType>
|
||||
void
|
||||
add_owned_msg_to_buffer(
|
||||
std::unique_ptr<MessageT, Deleter> message,
|
||||
uint64_t subscription_id,
|
||||
typename allocator::AllocRebind<MessageT, Alloc>::allocator_type & allocator)
|
||||
{
|
||||
add_owned_msg_to_buffers<MessageT, Alloc, Deleter, ROSMessageType>(
|
||||
std::move(message), {subscription_id}, allocator);
|
||||
}
|
||||
|
||||
/// Return true if the given rmw_gid_t matches any stored Publishers.
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
@@ -373,11 +306,6 @@ public:
|
||||
rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr
|
||||
get_subscription_intra_process(uint64_t intra_process_subscription_id);
|
||||
|
||||
/// Return the lowest available capacity for all subscription buffers for a publisher id.
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
lowest_available_capacity(const uint64_t intra_process_publisher_id) const;
|
||||
|
||||
private:
|
||||
struct SplittedSubscriptions
|
||||
{
|
||||
@@ -391,9 +319,6 @@ private:
|
||||
using PublisherMap =
|
||||
std::unordered_map<uint64_t, rclcpp::PublisherBase::WeakPtr>;
|
||||
|
||||
using PublisherBufferMap =
|
||||
std::unordered_map<uint64_t, rclcpp::experimental::buffers::IntraProcessBufferBase::WeakPtr>;
|
||||
|
||||
using PublisherToSubscriptionIdsMap =
|
||||
std::unordered_map<uint64_t, SplittedSubscriptions>;
|
||||
|
||||
@@ -412,54 +337,6 @@ private:
|
||||
rclcpp::PublisherBase::SharedPtr pub,
|
||||
rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const;
|
||||
|
||||
template<
|
||||
typename ROSMessageType,
|
||||
typename Alloc = std::allocator<ROSMessageType>
|
||||
>
|
||||
void do_transient_local_publish(
|
||||
const uint64_t pub_id, const uint64_t sub_id,
|
||||
const bool use_take_shared_method)
|
||||
{
|
||||
using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, Alloc>;
|
||||
using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
|
||||
using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;
|
||||
|
||||
auto publisher_buffer = publisher_buffers_[pub_id].lock();
|
||||
if (!publisher_buffer) {
|
||||
throw std::runtime_error("publisher buffer has unexpectedly gone out of scope");
|
||||
}
|
||||
auto buffer = std::dynamic_pointer_cast<
|
||||
rclcpp::experimental::buffers::IntraProcessBuffer<
|
||||
ROSMessageType,
|
||||
ROSMessageTypeAllocator,
|
||||
ROSMessageTypeDeleter
|
||||
>
|
||||
>(publisher_buffer);
|
||||
if (!buffer) {
|
||||
throw std::runtime_error(
|
||||
"failed to dynamic cast publisher's IntraProcessBufferBase to "
|
||||
"IntraProcessBuffer<ROSMessageType,ROSMessageTypeAllocator,"
|
||||
"ROSMessageTypeDeleter> which can happen when the publisher and "
|
||||
"subscription use different allocator types, which is not supported");
|
||||
}
|
||||
if (use_take_shared_method) {
|
||||
auto data_vec = buffer->get_all_data_shared();
|
||||
for (auto shared_data : data_vec) {
|
||||
this->template add_shared_msg_to_buffer<
|
||||
ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
|
||||
shared_data, sub_id);
|
||||
}
|
||||
} else {
|
||||
auto data_vec = buffer->get_all_data_unique();
|
||||
for (auto & owned_data : data_vec) {
|
||||
auto allocator = ROSMessageTypeAllocator();
|
||||
this->template add_owned_msg_to_buffer<
|
||||
ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>(
|
||||
std::move(owned_data), sub_id, allocator);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template<
|
||||
typename MessageT,
|
||||
typename Alloc,
|
||||
@@ -585,7 +462,7 @@ private:
|
||||
auto ptr = MessageAllocTraits::allocate(allocator, 1);
|
||||
MessageAllocTraits::construct(allocator, ptr, *message);
|
||||
|
||||
subscription->provide_intra_process_data(MessageUniquePtr(ptr, deleter));
|
||||
subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter)));
|
||||
}
|
||||
|
||||
continue;
|
||||
@@ -604,13 +481,13 @@ private:
|
||||
"subscription use different allocator types, which is not supported");
|
||||
}
|
||||
|
||||
if constexpr (rclcpp::TypeAdapter<MessageT, ROSMessageType>::is_specialized::value) {
|
||||
if constexpr (rclcpp::TypeAdapter<MessageT>::is_specialized::value) {
|
||||
ROSMessageTypeAllocator ros_message_alloc(allocator);
|
||||
auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1);
|
||||
ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr);
|
||||
auto ptr = ros_message_alloc.allocate(1);
|
||||
ros_message_alloc.construct(ptr);
|
||||
ROSMessageTypeDeleter deleter;
|
||||
allocator::set_allocator_for_deleter(&deleter, &allocator);
|
||||
rclcpp::TypeAdapter<MessageT, ROSMessageType>::convert_to_ros_message(*message, *ptr);
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*message, *ptr);
|
||||
auto ros_msg = std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, deleter);
|
||||
ros_message_subscription->provide_intra_process_message(std::move(ros_msg));
|
||||
} else {
|
||||
@@ -628,7 +505,7 @@ private:
|
||||
MessageAllocTraits::construct(allocator, ptr, *message);
|
||||
|
||||
ros_message_subscription->provide_intra_process_message(
|
||||
MessageUniquePtr(ptr, deleter));
|
||||
std::move(MessageUniquePtr(ptr, deleter)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -638,7 +515,6 @@ private:
|
||||
PublisherToSubscriptionIdsMap pub_to_subs_;
|
||||
SubscriptionMap subscriptions_;
|
||||
PublisherMap publishers_;
|
||||
PublisherBufferMap publisher_buffers_;
|
||||
|
||||
mutable std::shared_timed_mutex mutex_;
|
||||
};
|
||||
|
||||
@@ -87,7 +87,7 @@ public:
|
||||
buffer_type),
|
||||
any_callback_(callback)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_subscription_callback_added,
|
||||
static_cast<const void *>(this),
|
||||
static_cast<const void *>(&any_callback_));
|
||||
@@ -118,13 +118,6 @@ public:
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
if (this->buffer_->has_data()) {
|
||||
// If there is data still to be processed, indicate to the
|
||||
// executor or waitset by triggering the guard condition.
|
||||
this->trigger_guard_condition();
|
||||
}
|
||||
|
||||
return std::static_pointer_cast<void>(
|
||||
std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
|
||||
std::pair<ConstMessageSharedPtr, MessageUniquePtr>(
|
||||
|
||||
@@ -62,15 +62,6 @@ public:
|
||||
void
|
||||
add_to_wait_set(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
size_t
|
||||
available_capacity() const = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_durability_transient_local() const;
|
||||
|
||||
bool
|
||||
is_ready(rcl_wait_set_t * wait_set) override = 0;
|
||||
|
||||
|
||||
@@ -31,8 +31,6 @@
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/type_support_decl.hpp"
|
||||
|
||||
#include "tracetools/tracetools.h"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
@@ -93,10 +91,6 @@ public:
|
||||
buffer_type,
|
||||
qos_profile,
|
||||
std::make_shared<Alloc>(subscribed_type_allocator_));
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_ipb_to_subscription,
|
||||
static_cast<const void *>(buffer_.get()),
|
||||
static_cast<const void *>(this));
|
||||
}
|
||||
|
||||
bool
|
||||
@@ -169,11 +163,6 @@ public:
|
||||
return buffer_->use_take_shared_method();
|
||||
}
|
||||
|
||||
size_t available_capacity() const override
|
||||
{
|
||||
return buffer_->available_capacity();
|
||||
}
|
||||
|
||||
protected:
|
||||
void
|
||||
trigger_guard_condition() override
|
||||
|
||||
@@ -1,555 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__TIMERS_MANAGER_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include "rclcpp/context.hpp"
|
||||
#include "rclcpp/timer.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief This class provides a way for storing and executing timer objects.
|
||||
* It provides APIs to suit the needs of different applications and execution models.
|
||||
* All public APIs provided by this class are thread-safe.
|
||||
*
|
||||
* Timers management
|
||||
* This class provides APIs to add/remove timers to/from an internal storage.
|
||||
* It keeps a list of weak pointers from added timers, and locks them only when
|
||||
* they need to be executed or modified.
|
||||
* Timers are kept ordered in a binary-heap priority queue.
|
||||
* Calls to add/remove APIs will temporarily block the execution of the timers and
|
||||
* will require to reorder the internal priority queue.
|
||||
* Because of this, they have a not-negligible impact on the performance.
|
||||
*
|
||||
* Timers execution
|
||||
* The most efficient use of this class consists in letting a TimersManager object
|
||||
* to spawn a thread where timers are monitored and optionally executed.
|
||||
* This can be controlled via the `start` and `stop` methods.
|
||||
* Ready timers can either be executed or an on_ready_callback can be used to notify
|
||||
* other entities that they are ready and need to be executed.
|
||||
* Other APIs allow to directly execute a given timer.
|
||||
*
|
||||
* This class assumes that the `execute_callback()` API of the stored timers is never
|
||||
* called by other entities, but it can only be called from here.
|
||||
* If this assumption is not respected, the heap property may be invalidated,
|
||||
* so timers may be executed out of order, without this object noticing it.
|
||||
*
|
||||
*/
|
||||
class TimersManager
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimersManager)
|
||||
|
||||
/**
|
||||
* @brief Construct a new TimersManager object
|
||||
*
|
||||
* @param context custom context to be used.
|
||||
* Shared ownership of the context is held until destruction.
|
||||
* @param on_ready_callback The timers on ready callback. The presence of this function
|
||||
* indicates what to do when the TimersManager is running and a timer becomes ready.
|
||||
* The TimersManager is considered "running" when the `start` method has been called.
|
||||
* If it's callable, it will be invoked instead of the timer callback.
|
||||
* If it's not callable, then the TimersManager will
|
||||
* directly execute timers when they are ready.
|
||||
* All the methods that execute a given timer (e.g. `execute_head_timer`
|
||||
* or `execute_ready_timer`) without the TimersManager being `running`, i.e.
|
||||
* without actually explicitly waiting for the timer to become ready, will ignore this
|
||||
* callback.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
TimersManager(
|
||||
std::shared_ptr<rclcpp::Context> context,
|
||||
std::function<void(const rclcpp::TimerBase *)> on_ready_callback = nullptr);
|
||||
|
||||
/**
|
||||
* @brief Destruct the TimersManager object making sure to stop thread and release memory.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
~TimersManager();
|
||||
|
||||
/**
|
||||
* @brief Adds a new timer to the storage, maintaining weak ownership of it.
|
||||
* Function is thread safe and it can be called regardless of the state of the timers thread.
|
||||
*
|
||||
* @param timer the timer to add.
|
||||
* @throws std::invalid_argument if timer is a nullptr.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void add_timer(rclcpp::TimerBase::SharedPtr timer);
|
||||
|
||||
/**
|
||||
* @brief Remove a single timer from the object storage.
|
||||
* Will do nothing if the timer was not being stored here.
|
||||
* Function is thread safe and it can be called regardless of the state of the timers thread.
|
||||
*
|
||||
* @param timer the timer to remove.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void remove_timer(rclcpp::TimerBase::SharedPtr timer);
|
||||
|
||||
/**
|
||||
* @brief Remove all the timers stored in the object.
|
||||
* Function is thread safe and it can be called regardless of the state of the timers thread.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void clear();
|
||||
|
||||
/**
|
||||
* @brief Starts a thread that takes care of executing the timers stored in this object.
|
||||
* Function will throw an error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void start();
|
||||
|
||||
/**
|
||||
* @brief Stops the timers thread.
|
||||
* Will do nothing if the timer thread was not running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* @brief Get the number of timers that are currently ready.
|
||||
* This function is thread safe.
|
||||
*
|
||||
* @return size_t number of ready timers.
|
||||
* @throws std::runtime_error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t get_number_ready_timers();
|
||||
|
||||
/**
|
||||
* @brief Executes head timer if ready.
|
||||
* This function is thread safe.
|
||||
* This function will try to execute the timer callback regardless of whether
|
||||
* the TimersManager on_ready_callback was passed during construction.
|
||||
*
|
||||
* @return true if head timer was ready.
|
||||
* @throws std::runtime_error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool execute_head_timer();
|
||||
|
||||
/**
|
||||
* @brief Executes timer identified by its ID.
|
||||
* This function is thread safe.
|
||||
* This function will try to execute the timer callback regardless of whether
|
||||
* the TimersManager on_ready_callback was passed during construction.
|
||||
*
|
||||
* @param timer_id the timer ID of the timer to execute
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void execute_ready_timer(const rclcpp::TimerBase * timer_id);
|
||||
|
||||
/**
|
||||
* @brief Get the amount of time before the next timer triggers.
|
||||
* This function is thread safe.
|
||||
*
|
||||
* @return std::optional<std::chrono::nanoseconds> to wait,
|
||||
* the returned value could be negative if the timer is already expired
|
||||
* or std::chrono::nanoseconds::max() if there are no timers stored in the object.
|
||||
* If the head timer was cancelled, then this will return a nullopt.
|
||||
* @throws std::runtime_error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::optional<std::chrono::nanoseconds> get_head_timeout();
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(TimersManager)
|
||||
|
||||
using TimerPtr = rclcpp::TimerBase::SharedPtr;
|
||||
using WeakTimerPtr = rclcpp::TimerBase::WeakPtr;
|
||||
|
||||
// Forward declaration
|
||||
class TimersHeap;
|
||||
|
||||
/**
|
||||
* @brief This class allows to store weak pointers to timers in a heap-like data structure.
|
||||
* The root of the heap is the timer that triggers first.
|
||||
* Since this class uses weak ownership, it is not guaranteed that it represents a valid heap
|
||||
* at any point in time as timers could go out of scope, thus invalidating it.
|
||||
* The "validate_and_lock" API allows to restore the heap property and also returns a locked version
|
||||
* of the timers heap.
|
||||
* This class is not thread safe and requires external mutexes to protect its usage.
|
||||
*/
|
||||
class WeakTimersHeap
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Add a new timer to the heap. After the addition, the heap property is enforced.
|
||||
*
|
||||
* @param timer new timer to add.
|
||||
* @return true if timer has been added, false if it was already there.
|
||||
*/
|
||||
bool add_timer(TimerPtr timer)
|
||||
{
|
||||
TimersHeap locked_heap = this->validate_and_lock();
|
||||
bool added = locked_heap.add_timer(std::move(timer));
|
||||
|
||||
if (added) {
|
||||
// Re-create the weak heap with the new timer added
|
||||
this->store(locked_heap);
|
||||
}
|
||||
|
||||
return added;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Remove a timer from the heap. After the removal, the heap property is enforced.
|
||||
*
|
||||
* @param timer timer to remove.
|
||||
* @return true if timer has been removed, false if it was not there.
|
||||
*/
|
||||
bool remove_timer(TimerPtr timer)
|
||||
{
|
||||
TimersHeap locked_heap = this->validate_and_lock();
|
||||
bool removed = locked_heap.remove_timer(std::move(timer));
|
||||
|
||||
if (removed) {
|
||||
// Re-create the weak heap with the timer removed
|
||||
this->store(locked_heap);
|
||||
}
|
||||
|
||||
return removed;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Retrieve the timer identified by the key
|
||||
* @param timer_id The ID of the timer to retrieve.
|
||||
* @return TimerPtr if there's a timer associated with the ID, nullptr otherwise
|
||||
*/
|
||||
TimerPtr get_timer(const rclcpp::TimerBase * timer_id)
|
||||
{
|
||||
for (auto & weak_timer : weak_heap_) {
|
||||
auto timer = weak_timer.lock();
|
||||
if (timer.get() == timer_id) {
|
||||
return timer;
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a const reference to the front element.
|
||||
*/
|
||||
const WeakTimerPtr & front() const
|
||||
{
|
||||
return weak_heap_.front();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns whether the heap is empty or not.
|
||||
*/
|
||||
bool empty() const
|
||||
{
|
||||
return weak_heap_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function restores the current object as a valid heap
|
||||
* and it returns a locked version of it.
|
||||
* Timers that went out of scope are removed from the container.
|
||||
* It is the only public API to access and manipulate the stored timers.
|
||||
*
|
||||
* @return TimersHeap owned timers corresponding to the current object
|
||||
*/
|
||||
TimersHeap validate_and_lock()
|
||||
{
|
||||
TimersHeap locked_heap;
|
||||
bool any_timer_destroyed = false;
|
||||
|
||||
for (auto weak_timer : weak_heap_) {
|
||||
auto timer = weak_timer.lock();
|
||||
if (timer) {
|
||||
// This timer is valid, so add it to the locked heap
|
||||
// Note: we access friend private `owned_heap_` member field.
|
||||
locked_heap.owned_heap_.push_back(std::move(timer));
|
||||
} else {
|
||||
// This timer went out of scope, so we don't add it to locked heap
|
||||
// and we mark the corresponding flag.
|
||||
// It's not needed to erase it from weak heap, as we are going to re-heapify.
|
||||
// Note: we can't exit from the loop here, as we need to find all valid timers.
|
||||
any_timer_destroyed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// If a timer has gone out of scope, then the remaining elements do not represent
|
||||
// a valid heap anymore. We need to re-heapify the timers heap.
|
||||
if (any_timer_destroyed) {
|
||||
locked_heap.heapify();
|
||||
// Re-create the weak heap now that elements have been heapified again
|
||||
this->store(locked_heap);
|
||||
}
|
||||
|
||||
return locked_heap;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function allows to recreate the heap of weak pointers
|
||||
* from an heap of owned pointers.
|
||||
* It is required to be called after a locked TimersHeap generated from this object
|
||||
* has been modified in any way (e.g. timers triggered, added, removed).
|
||||
*
|
||||
* @param heap timers heap to store as weak pointers
|
||||
*/
|
||||
void store(const TimersHeap & heap)
|
||||
{
|
||||
weak_heap_.clear();
|
||||
// Note: we access friend private `owned_heap_` member field.
|
||||
for (auto t : heap.owned_heap_) {
|
||||
weak_heap_.push_back(t);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Remove all timers from the heap.
|
||||
*/
|
||||
void clear()
|
||||
{
|
||||
weak_heap_.clear();
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<WeakTimerPtr> weak_heap_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This class is the equivalent of WeakTimersHeap but with ownership of the timers.
|
||||
* It can be generated by locking the weak version.
|
||||
* It provides operations to manipulate the heap.
|
||||
* This class is not thread safe and requires external mutexes to protect its usage.
|
||||
*/
|
||||
class TimersHeap
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Try to add a new timer to the heap.
|
||||
* After the addition, the heap property is preserved.
|
||||
* @param timer new timer to add.
|
||||
* @return true if timer has been added, false if it was already there.
|
||||
*/
|
||||
bool add_timer(TimerPtr timer)
|
||||
{
|
||||
// Nothing to do if the timer is already stored here
|
||||
auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
|
||||
if (it != owned_heap_.end()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
owned_heap_.push_back(std::move(timer));
|
||||
std::push_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Try to remove a timer from the heap.
|
||||
* After the removal, the heap property is preserved.
|
||||
* @param timer timer to remove.
|
||||
* @return true if timer has been removed, false if it was not there.
|
||||
*/
|
||||
bool remove_timer(TimerPtr timer)
|
||||
{
|
||||
// Nothing to do if the timer is not stored here
|
||||
auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
|
||||
if (it == owned_heap_.end()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
owned_heap_.erase(it);
|
||||
this->heapify();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a reference to the front element.
|
||||
* @return reference to front element.
|
||||
*/
|
||||
TimerPtr & front()
|
||||
{
|
||||
return owned_heap_.front();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a const reference to the front element.
|
||||
* @return const reference to front element.
|
||||
*/
|
||||
const TimerPtr & front() const
|
||||
{
|
||||
return owned_heap_.front();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns whether the heap is empty or not.
|
||||
* @return true if the heap is empty.
|
||||
*/
|
||||
bool empty() const
|
||||
{
|
||||
return owned_heap_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the size of the heap.
|
||||
* @return the number of valid timers in the heap.
|
||||
*/
|
||||
size_t size() const
|
||||
{
|
||||
return owned_heap_.size();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the number of timers that are currently ready.
|
||||
* @return size_t number of ready timers.
|
||||
*/
|
||||
size_t get_number_ready_timers() const
|
||||
{
|
||||
size_t ready_timers = 0;
|
||||
|
||||
for (TimerPtr t : owned_heap_) {
|
||||
if (t->is_ready()) {
|
||||
ready_timers++;
|
||||
}
|
||||
}
|
||||
|
||||
return ready_timers;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Restore a valid heap after the root value has been replaced (e.g. timer triggered).
|
||||
*/
|
||||
void heapify_root()
|
||||
{
|
||||
// The following code is a more efficient version than doing
|
||||
// pop_heap, pop_back, push_back, push_heap
|
||||
// as it removes the need for the last push_heap
|
||||
|
||||
// Push the modified element (i.e. the current root) at the bottom of the heap
|
||||
owned_heap_.push_back(owned_heap_[0]);
|
||||
// Exchange first and last-1 elements and reheapify
|
||||
std::pop_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
|
||||
// Remove last element
|
||||
owned_heap_.pop_back();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Completely restores the structure to a valid heap
|
||||
*/
|
||||
void heapify()
|
||||
{
|
||||
std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper function to clear the "on_reset_callback" on all associated timers.
|
||||
*/
|
||||
void clear_timers_on_reset_callbacks()
|
||||
{
|
||||
for (TimerPtr & t : owned_heap_) {
|
||||
t->clear_on_reset_callback();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Friend declaration to allow the `validate_and_lock()` function to access the
|
||||
* underlying heap container
|
||||
*/
|
||||
friend TimersHeap WeakTimersHeap::validate_and_lock();
|
||||
|
||||
/**
|
||||
* @brief Friend declaration to allow the `store()` function to access the
|
||||
* underlying heap container
|
||||
*/
|
||||
friend void WeakTimersHeap::store(const TimersHeap & heap);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Comparison function between timers.
|
||||
* @return true if `a` triggers after `b`.
|
||||
*/
|
||||
static bool timer_greater(TimerPtr a, TimerPtr b)
|
||||
{
|
||||
// TODO(alsora): this can cause an error if timers are using different clocks
|
||||
return a->time_until_trigger() > b->time_until_trigger();
|
||||
}
|
||||
|
||||
std::vector<TimerPtr> owned_heap_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Implements a loop that keeps executing ready timers.
|
||||
* This function is executed in the timers thread.
|
||||
*/
|
||||
void run_timers();
|
||||
|
||||
/**
|
||||
* @brief Get the amount of time before the next timer triggers.
|
||||
* This function is not thread safe, acquire a mutex before calling it.
|
||||
*
|
||||
* @return std::optional<std::chrono::nanoseconds> to wait,
|
||||
* the returned value could be negative if the timer is already expired
|
||||
* or std::chrono::nanoseconds::max() if the heap is empty.
|
||||
* If the head timer was cancelled, then this will return a nullopt.
|
||||
* This function is not thread safe, acquire the timers_mutex_ before calling it.
|
||||
*/
|
||||
std::optional<std::chrono::nanoseconds> get_head_timeout_unsafe();
|
||||
|
||||
/**
|
||||
* @brief Executes all the timers currently ready when the function is invoked
|
||||
* while keeping the heap correctly sorted.
|
||||
* This function is not thread safe, acquire the timers_mutex_ before calling it.
|
||||
*/
|
||||
void execute_ready_timers_unsafe();
|
||||
|
||||
// Callback to be called when timer is ready
|
||||
std::function<void(const rclcpp::TimerBase *)> on_ready_callback_;
|
||||
|
||||
// Thread used to run the timers execution task
|
||||
std::thread timers_thread_;
|
||||
// Protects access to timers
|
||||
std::mutex timers_mutex_;
|
||||
// Protects access to stop()
|
||||
std::mutex stop_mutex_;
|
||||
// Notifies the timers thread whenever timers are added/removed
|
||||
std::condition_variable timers_cv_;
|
||||
// Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed
|
||||
bool timers_updated_ {false};
|
||||
// Indicates whether the timers thread is currently running or not
|
||||
std::atomic<bool> running_ {false};
|
||||
// Parent context used to understand if ROS is still active
|
||||
std::shared_ptr<rclcpp::Context> context_;
|
||||
// Timers heap storage with weak ownership
|
||||
WeakTimersHeap weak_timers_heap_;
|
||||
};
|
||||
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
|
||||
@@ -1,207 +0,0 @@
|
||||
// Copyright 2023 Sony Group Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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__GENERIC_CLIENT_HPP_
|
||||
#define RCLCPP__GENERIC_CLIENT_HPP_
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <future>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include "rcl/client.h"
|
||||
|
||||
#include "rclcpp/client.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
#include "rcpputils/shared_library.hpp"
|
||||
|
||||
#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
class GenericClient : public ClientBase
|
||||
{
|
||||
public:
|
||||
using Request = void *; // Serialized data pointer of request message
|
||||
using Response = void *; // Serialized data pointer of response message
|
||||
|
||||
using SharedResponse = std::shared_ptr<void>;
|
||||
|
||||
using Promise = std::promise<SharedResponse>;
|
||||
using SharedPromise = std::shared_ptr<Promise>;
|
||||
|
||||
using Future = std::future<SharedResponse>;
|
||||
using SharedFuture = std::shared_future<SharedResponse>;
|
||||
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(GenericClient)
|
||||
|
||||
/// A convenient GenericClient::Future and request id pair.
|
||||
/**
|
||||
* Public members:
|
||||
* - future: a std::future<void *>.
|
||||
* - request_id: the request id associated with the future.
|
||||
*
|
||||
* All the other methods are equivalent to the ones std::future provides.
|
||||
*/
|
||||
struct FutureAndRequestId
|
||||
: detail::FutureAndRequestId<Future>
|
||||
{
|
||||
using detail::FutureAndRequestId<Future>::FutureAndRequestId;
|
||||
|
||||
/// See std::future::share().
|
||||
SharedFuture share() noexcept {return this->future.share();}
|
||||
|
||||
/// Move constructor.
|
||||
FutureAndRequestId(FutureAndRequestId && other) noexcept = default;
|
||||
/// Deleted copy constructor, each instance is a unique owner of the future.
|
||||
FutureAndRequestId(const FutureAndRequestId & other) = delete;
|
||||
/// Move assignment.
|
||||
FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default;
|
||||
/// Deleted copy assignment, each instance is a unique owner of the future.
|
||||
FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete;
|
||||
/// Destructor.
|
||||
~FutureAndRequestId() = default;
|
||||
};
|
||||
|
||||
GenericClient(
|
||||
rclcpp::node_interfaces::NodeBaseInterface * node_base,
|
||||
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
|
||||
const std::string & service_name,
|
||||
const std::string & service_type,
|
||||
rcl_client_options_t & client_options);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
SharedResponse
|
||||
create_response() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<rmw_request_id_t>
|
||||
create_request_header() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
handle_response(
|
||||
std::shared_ptr<rmw_request_id_t> request_header,
|
||||
std::shared_ptr<void> response) override;
|
||||
|
||||
/// Send a request to the service server.
|
||||
/**
|
||||
* This method returns a `FutureAndRequestId` instance
|
||||
* that can be passed to Executor::spin_until_future_complete() to
|
||||
* wait until it has been completed.
|
||||
*
|
||||
* If the future never completes,
|
||||
* e.g. the call to Executor::spin_until_future_complete() times out,
|
||||
* GenericClient::remove_pending_request() must be called to clean the client internal state.
|
||||
* Not doing so will make the `Client` instance to use more memory each time a response is not
|
||||
* received from the service server.
|
||||
*
|
||||
* ```cpp
|
||||
* auto future = client->async_send_request(my_request);
|
||||
* if (
|
||||
* rclcpp::FutureReturnCode::TIMEOUT ==
|
||||
* executor->spin_until_future_complete(future, timeout))
|
||||
* {
|
||||
* client->remove_pending_request(future);
|
||||
* // handle timeout
|
||||
* } else {
|
||||
* handle_response(future.get());
|
||||
* }
|
||||
* ```
|
||||
*
|
||||
* \param[in] request request to be send.
|
||||
* \return a FutureAndRequestId instance.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
FutureAndRequestId
|
||||
async_send_request(const Request request);
|
||||
|
||||
/// Clean all pending requests older than a time_point.
|
||||
/**
|
||||
* \param[in] time_point Requests that were sent before this point are going to be removed.
|
||||
* \param[inout] pruned_requests Removed requests id will be pushed to the vector
|
||||
* if a pointer is provided.
|
||||
* \return number of pending requests that were removed.
|
||||
*/
|
||||
template<typename AllocatorT = std::allocator<int64_t>>
|
||||
size_t
|
||||
prune_requests_older_than(
|
||||
std::chrono::time_point<std::chrono::system_clock> time_point,
|
||||
std::vector<int64_t, AllocatorT> * pruned_requests = nullptr)
|
||||
{
|
||||
return detail::prune_requests_older_than_impl(
|
||||
pending_requests_,
|
||||
pending_requests_mutex_,
|
||||
time_point,
|
||||
pruned_requests);
|
||||
}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
prune_pending_requests();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
remove_pending_request(
|
||||
int64_t request_id);
|
||||
|
||||
/// Take the next response for this client.
|
||||
/**
|
||||
* \sa ClientBase::take_type_erased_response().
|
||||
*
|
||||
* \param[out] response_out The reference to a Service Response into
|
||||
* which the middleware will copy the response being taken.
|
||||
* \param[out] request_header_out The request header to be filled by the
|
||||
* middleware when taking, and which can be used to associate the response
|
||||
* to a specific request.
|
||||
* \returns true if the response was taken, otherwise false.
|
||||
* \throws rclcpp::exceptions::RCLError based exceptions if the underlying
|
||||
* rcl function fail.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
take_response(Response response_out, rmw_request_id_t & request_header_out)
|
||||
{
|
||||
return this->take_type_erased_response(response_out, request_header_out);
|
||||
}
|
||||
|
||||
protected:
|
||||
using CallbackInfoVariant = std::variant<
|
||||
std::promise<SharedResponse>>; // Use variant for extension
|
||||
|
||||
int64_t
|
||||
async_send_request_impl(
|
||||
const Request request,
|
||||
CallbackInfoVariant value);
|
||||
|
||||
std::optional<CallbackInfoVariant>
|
||||
get_and_erase_pending_request(
|
||||
int64_t request_number);
|
||||
|
||||
RCLCPP_DISABLE_COPY(GenericClient)
|
||||
|
||||
std::map<int64_t, std::pair<
|
||||
std::chrono::time_point<std::chrono::system_clock>,
|
||||
CallbackInfoVariant>> pending_requests_;
|
||||
std::mutex pending_requests_mutex_;
|
||||
|
||||
private:
|
||||
std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
|
||||
const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_;
|
||||
};
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__GENERIC_CLIENT_HPP_
|
||||
@@ -77,7 +77,7 @@ public:
|
||||
: rclcpp::PublisherBase(
|
||||
node_base,
|
||||
topic_name,
|
||||
*rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
|
||||
*rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
|
||||
options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos),
|
||||
// NOTE(methylDragon): Passing these args separately is necessary for event binding
|
||||
options.event_callbacks,
|
||||
|
||||
@@ -74,21 +74,18 @@ public:
|
||||
const std::string & topic_name,
|
||||
const std::string & topic_type,
|
||||
const rclcpp::QoS & qos,
|
||||
AnySubscriptionCallback<rclcpp::SerializedMessage, AllocatorT> callback,
|
||||
// TODO(nnmm): Add variant for callback with message info. See issue #1604.
|
||||
std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
|
||||
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
|
||||
: SubscriptionBase(
|
||||
node_base,
|
||||
*rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
|
||||
*rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
|
||||
topic_name,
|
||||
options.to_rcl_subscription_options(qos),
|
||||
options.event_callbacks,
|
||||
options.use_default_callbacks,
|
||||
DeliveredMessageKind::SERIALIZED_MESSAGE),
|
||||
callback_([callback](
|
||||
std::shared_ptr<const rclcpp::SerializedMessage> serialized_message,
|
||||
const rclcpp::MessageInfo & message_info) mutable {
|
||||
callback.dispatch(serialized_message, message_info);
|
||||
}),
|
||||
SubscriptionType::SERIALIZED_MESSAGE),
|
||||
callback_(callback),
|
||||
ts_lib_(ts_lib)
|
||||
{}
|
||||
|
||||
@@ -128,6 +125,7 @@ public:
|
||||
|
||||
|
||||
// DYNAMIC TYPE ==================================================================================
|
||||
// TODO(methylDragon): Reorder later
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type()
|
||||
override;
|
||||
@@ -154,9 +152,7 @@ public:
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(GenericSubscription)
|
||||
|
||||
std::function<void(
|
||||
std::shared_ptr<const rclcpp::SerializedMessage>,
|
||||
const rclcpp::MessageInfo)> callback_;
|
||||
std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback_;
|
||||
// The type support library should stay loaded, so it is stored in the GenericSubscription
|
||||
std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
|
||||
};
|
||||
|
||||
@@ -48,7 +48,7 @@ public:
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit GuardCondition(
|
||||
const rclcpp::Context::SharedPtr & context =
|
||||
rclcpp::Context::SharedPtr context =
|
||||
rclcpp::contexts::get_global_default_context(),
|
||||
rcl_guard_condition_options_t guard_condition_options =
|
||||
rcl_guard_condition_get_default_options());
|
||||
@@ -57,6 +57,11 @@ public:
|
||||
virtual
|
||||
~GuardCondition();
|
||||
|
||||
/// Return the context used when creating this guard condition.
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::Context::SharedPtr
|
||||
get_context() const;
|
||||
|
||||
/// Return the underlying rcl guard condition structure.
|
||||
RCLCPP_PUBLIC
|
||||
rcl_guard_condition_t &
|
||||
@@ -123,6 +128,7 @@ public:
|
||||
set_on_trigger_callback(std::function<void(size_t)> callback);
|
||||
|
||||
protected:
|
||||
rclcpp::Context::SharedPtr context_;
|
||||
rcl_guard_condition_t rcl_guard_condition_;
|
||||
std::atomic<bool> in_use_by_wait_set_{false};
|
||||
std::recursive_mutex reentrant_mutex_;
|
||||
|
||||
@@ -126,6 +126,9 @@ private:
|
||||
std::shared_ptr<std::pair<std::string, std::string>> logger_sublogger_pairname_ = nullptr;
|
||||
|
||||
public:
|
||||
RCLCPP_PUBLIC
|
||||
Logger(const Logger &) = default;
|
||||
|
||||
/// Get the name of this logger.
|
||||
/**
|
||||
* \return the full name of the logger including any prefixes, or
|
||||
|
||||
@@ -42,7 +42,6 @@
|
||||
#include "rclcpp/clock.hpp"
|
||||
#include "rclcpp/context.hpp"
|
||||
#include "rclcpp/event.hpp"
|
||||
#include "rclcpp/generic_client.hpp"
|
||||
#include "rclcpp/generic_publisher.hpp"
|
||||
#include "rclcpp/generic_subscription.hpp"
|
||||
#include "rclcpp/logger.hpp"
|
||||
@@ -57,7 +56,6 @@
|
||||
#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_type_descriptions_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
|
||||
#include "rclcpp/node_options.hpp"
|
||||
#include "rclcpp/parameter.hpp"
|
||||
@@ -234,15 +232,13 @@ public:
|
||||
* \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.
|
||||
* \param[in] autostart The state of the clock on initialization.
|
||||
*/
|
||||
template<typename DurationRepT = int64_t, typename DurationT = std::milli, typename CallbackT>
|
||||
typename rclcpp::WallTimer<CallbackT>::SharedPtr
|
||||
create_wall_timer(
|
||||
std::chrono::duration<DurationRepT, DurationT> period,
|
||||
CallbackT callback,
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr,
|
||||
bool autostart = true);
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr);
|
||||
|
||||
/// Create a timer that uses the node clock to drive the callback.
|
||||
/**
|
||||
@@ -321,22 +317,6 @@ public:
|
||||
const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr);
|
||||
|
||||
/// Create and return a GenericClient.
|
||||
/**
|
||||
* \param[in] service_name The name on which the service is accessible.
|
||||
* \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool"
|
||||
* \param[in] qos Quality of service profile for client.
|
||||
* \param[in] group Callback group to handle the reply to service calls.
|
||||
* \return Shared pointer to the created GenericClient.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GenericClient::SharedPtr
|
||||
create_generic_client(
|
||||
const std::string & service_name,
|
||||
const std::string & service_type,
|
||||
const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
|
||||
rclcpp::CallbackGroup::SharedPtr group = nullptr);
|
||||
|
||||
/// Create and return a GenericPublisher.
|
||||
/**
|
||||
* The returned pointer will never be empty, but this function can throw various exceptions, for
|
||||
@@ -375,14 +355,12 @@ public:
|
||||
* `%callback_group`.
|
||||
* \return Shared pointer to the created generic subscription.
|
||||
*/
|
||||
template<
|
||||
typename CallbackT,
|
||||
typename AllocatorT = std::allocator<void>>
|
||||
template<typename AllocatorT = std::allocator<void>>
|
||||
std::shared_ptr<rclcpp::GenericSubscription> create_generic_subscription(
|
||||
const std::string & topic_name,
|
||||
const std::string & topic_type,
|
||||
const rclcpp::QoS & qos,
|
||||
CallbackT && callback,
|
||||
std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
|
||||
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
|
||||
rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
|
||||
)
|
||||
@@ -991,16 +969,7 @@ public:
|
||||
|
||||
/// Return a list of parameters with any of the given prefixes, up to the given depth.
|
||||
/**
|
||||
* Parameters are separated into a hierarchy using the "." (dot) character.
|
||||
* The "prefixes" argument is a way to select only particular parts of the hierarchy.
|
||||
*
|
||||
* \param[in] prefixes The list of prefixes that should be searched for within the
|
||||
* current parameters. If this vector of prefixes is empty, then list_parameters
|
||||
* will return all parameters.
|
||||
* \param[in] depth An unsigned integer that represents the recursive depth to search.
|
||||
* If this depth = 0, then all parameters that fit the prefixes will be returned.
|
||||
* \returns A ListParametersResult message which contains both an array of unique prefixes
|
||||
* and an array of names that were matched to the prefixes given.
|
||||
* \todo: properly document and test this method.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rcl_interfaces::msg::ListParametersResult
|
||||
@@ -1333,26 +1302,6 @@ public:
|
||||
size_t
|
||||
count_subscribers(const std::string & topic_name) const;
|
||||
|
||||
/// Return the number of clients created for a given service.
|
||||
/**
|
||||
* \param[in] service_name the actual service name used; it will not be automatically remapped.
|
||||
* \return number of clients that have been created for the given service.
|
||||
* \throws std::runtime_error if clients could not be counted
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
count_clients(const std::string & service_name) const;
|
||||
|
||||
/// Return the number of services created for a given service.
|
||||
/**
|
||||
* \param[in] service_name the actual service name used; it will not be automatically remapped.
|
||||
* \return number of services that have been created for the given service.
|
||||
* \throws std::runtime_error if services could not be counted
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
count_services(const std::string & service_name) const;
|
||||
|
||||
/// Return the topic endpoint information about publishers on a given topic.
|
||||
/**
|
||||
* The returned parameter is a list of topic endpoint information, where each item will contain
|
||||
@@ -1505,11 +1454,6 @@ public:
|
||||
rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
|
||||
get_node_time_source_interface();
|
||||
|
||||
/// Return the Node's internal NodeTypeDescriptionsInterface implementation.
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr
|
||||
get_node_type_descriptions_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
|
||||
@@ -1642,18 +1586,11 @@ private:
|
||||
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::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_;
|
||||
rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
|
||||
|
||||
const rclcpp::NodeOptions node_options_;
|
||||
const std::string sub_namespace_;
|
||||
const std::string effective_namespace_;
|
||||
|
||||
class NodeImpl;
|
||||
// This member is meant to be a place to backport features into stable distributions,
|
||||
// and new features targeting Rolling should not use this.
|
||||
// See the comment in node.cpp for more information.
|
||||
std::shared_ptr<NodeImpl> hidden_impl_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -110,16 +110,14 @@ typename rclcpp::WallTimer<CallbackT>::SharedPtr
|
||||
Node::create_wall_timer(
|
||||
std::chrono::duration<DurationRepT, DurationT> period,
|
||||
CallbackT callback,
|
||||
rclcpp::CallbackGroup::SharedPtr group,
|
||||
bool autostart)
|
||||
rclcpp::CallbackGroup::SharedPtr group)
|
||||
{
|
||||
return rclcpp::create_wall_timer(
|
||||
period,
|
||||
std::move(callback),
|
||||
group,
|
||||
this->node_base_.get(),
|
||||
this->node_timers_.get(),
|
||||
autostart);
|
||||
this->node_timers_.get());
|
||||
}
|
||||
|
||||
template<typename DurationRepT, typename DurationT, typename CallbackT>
|
||||
@@ -221,13 +219,13 @@ Node::create_generic_publisher(
|
||||
);
|
||||
}
|
||||
|
||||
template<typename CallbackT, typename AllocatorT>
|
||||
template<typename AllocatorT>
|
||||
std::shared_ptr<rclcpp::GenericSubscription>
|
||||
Node::create_generic_subscription(
|
||||
const std::string & topic_name,
|
||||
const std::string & topic_type,
|
||||
const rclcpp::QoS & qos,
|
||||
CallbackT && callback,
|
||||
std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)> callback,
|
||||
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
|
||||
{
|
||||
return rclcpp::create_generic_subscription(
|
||||
@@ -235,7 +233,7 @@ Node::create_generic_subscription(
|
||||
extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
|
||||
topic_type,
|
||||
qos,
|
||||
std::forward<CallbackT>(callback),
|
||||
std::move(callback),
|
||||
options
|
||||
);
|
||||
}
|
||||
|
||||
@@ -167,7 +167,6 @@ init_tuple(NodeT & n)
|
||||
* something like that, then you'll need to create your own specialization of
|
||||
* the NodeInterfacesSupports struct without this macro.
|
||||
*/
|
||||
// *INDENT-OFF*
|
||||
#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \
|
||||
namespace rclcpp::node_interfaces::detail { \
|
||||
template<typename StorageClassT, typename ... RemainingInterfaceTs> \
|
||||
@@ -190,7 +189,7 @@ init_tuple(NodeT & n)
|
||||
/* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
|
||||
template<typename ... ArgsT> \
|
||||
explicit NodeInterfacesSupports(ArgsT && ... args) \
|
||||
: NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
|
||||
: NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
|
||||
std::forward<ArgsT>(args) ...) \
|
||||
{} \
|
||||
\
|
||||
@@ -201,7 +200,6 @@ init_tuple(NodeT & n)
|
||||
} \
|
||||
}; \
|
||||
} // namespace rclcpp::node_interfaces::detail
|
||||
// *INDENT-ON*
|
||||
|
||||
} // namespace detail
|
||||
} // namespace node_interfaces
|
||||
|
||||
@@ -121,19 +121,10 @@ public:
|
||||
std::atomic_bool &
|
||||
get_associated_with_executor_atomic() override;
|
||||
|
||||
[[deprecated("Use get_shared_notify_guard_condition or trigger_notify_guard_condition instead")]]
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GuardCondition &
|
||||
get_notify_guard_condition() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
get_shared_notify_guard_condition() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
trigger_notify_guard_condition() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
get_use_intra_process_default() const override;
|
||||
@@ -162,7 +153,7 @@ private:
|
||||
|
||||
/// Guard condition for notifying the Executor of changes to this node.
|
||||
mutable std::recursive_mutex notify_guard_condition_mutex_;
|
||||
std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_;
|
||||
rclcpp::GuardCondition notify_guard_condition_;
|
||||
bool notify_guard_condition_is_valid_;
|
||||
};
|
||||
|
||||
|
||||
@@ -148,33 +148,13 @@ public:
|
||||
/**
|
||||
* For example, this should be notified when a publisher is added or removed.
|
||||
*
|
||||
* \return the GuardCondition if it is valid, else throw runtime error
|
||||
* \return the GuardCondition if it is valid, else thow runtime error
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::GuardCondition &
|
||||
get_notify_guard_condition() = 0;
|
||||
|
||||
/// Return a 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 GuardCondition if it is valid, else nullptr
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
get_shared_notify_guard_condition() = 0;
|
||||
|
||||
/// Trigger the guard condition that notifies of internal node state changes.
|
||||
/**
|
||||
* For example, this should be notified when a publisher is added or removed.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
void
|
||||
trigger_notify_guard_condition() = 0;
|
||||
|
||||
/// Return the default preference for using intra process communication.
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
|
||||
@@ -113,14 +113,6 @@ public:
|
||||
size_t
|
||||
count_subscribers(const std::string & topic_name) const override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
count_clients(const std::string & service_name) const override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
count_services(const std::string & service_name) const override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const rcl_guard_condition_t *
|
||||
get_graph_guard_condition() const override;
|
||||
|
||||
@@ -57,8 +57,7 @@ public:
|
||||
node_namespace_(info.node_namespace),
|
||||
topic_type_(info.topic_type),
|
||||
endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
|
||||
qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile),
|
||||
topic_type_hash_(info.topic_type_hash)
|
||||
qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
|
||||
{
|
||||
std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
|
||||
}
|
||||
@@ -123,16 +122,6 @@ public:
|
||||
const rclcpp::QoS &
|
||||
qos_profile() const;
|
||||
|
||||
/// Get a mutable reference to the type hash of the topic endpoint.
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_type_hash_t &
|
||||
topic_type_hash();
|
||||
|
||||
/// Get a const reference to the type hash of the topic endpoint.
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_type_hash_t &
|
||||
topic_type_hash() const;
|
||||
|
||||
private:
|
||||
std::string node_name_;
|
||||
std::string node_namespace_;
|
||||
@@ -140,7 +129,6 @@ private:
|
||||
rclcpp::EndpointType endpoint_type_;
|
||||
std::array<uint8_t, RMW_GID_STORAGE_SIZE> endpoint_gid_;
|
||||
rclcpp::QoS qos_profile_;
|
||||
rosidl_type_hash_t topic_type_hash_;
|
||||
};
|
||||
|
||||
namespace node_interfaces
|
||||
@@ -305,24 +293,6 @@ public:
|
||||
size_t
|
||||
count_subscribers(const std::string & topic_name) const = 0;
|
||||
|
||||
/// Return the number of clients created for a given service.
|
||||
/*
|
||||
* \param[in] service_name the actual service name used; it will not be automatically remapped.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
size_t
|
||||
count_clients(const std::string & service_name) const = 0;
|
||||
|
||||
/// Return the number of services created for a given service.
|
||||
/*
|
||||
* \param[in] service_name the actual service name used; it will not be automatically remapped.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
size_t
|
||||
count_services(const std::string & service_name) const = 0;
|
||||
|
||||
/// Return the rcl guard condition which is triggered when the ROS graph changes.
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
|
||||
@@ -30,7 +30,6 @@
|
||||
rclcpp::node_interfaces::NodeTimeSourceInterface, \
|
||||
rclcpp::node_interfaces::NodeTimersInterface, \
|
||||
rclcpp::node_interfaces::NodeTopicsInterface, \
|
||||
rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \
|
||||
rclcpp::node_interfaces::NodeWaitablesInterface
|
||||
|
||||
|
||||
@@ -119,7 +118,6 @@ public:
|
||||
* - rclcpp::node_interfaces::NodeTimeSourceInterface
|
||||
* - rclcpp::node_interfaces::NodeTimersInterface
|
||||
* - rclcpp::node_interfaces::NodeTopicsInterface
|
||||
* - rclcpp::node_interfaces::NodeTypeDescriptionsInterface
|
||||
* - rclcpp::node_interfaces::NodeWaitablesInterface
|
||||
*
|
||||
* Or you use custom interfaces as long as you make a template specialization
|
||||
@@ -127,9 +125,7 @@ public:
|
||||
* the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
|
||||
*
|
||||
* Usage example:
|
||||
* ```cpp
|
||||
* RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
|
||||
* ```
|
||||
* ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)```
|
||||
*
|
||||
* If you choose not to use the helper macro, then you can specialize the
|
||||
* template yourself, but you must:
|
||||
|
||||
@@ -23,9 +23,6 @@
|
||||
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
#include "rcl_interfaces/srv/get_logger_levels.hpp"
|
||||
#include "rcl_interfaces/srv/set_logger_levels.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace node_interfaces
|
||||
@@ -38,7 +35,7 @@ public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
|
||||
explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
@@ -52,21 +49,13 @@ public:
|
||||
const char *
|
||||
get_logger_name() const override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
create_logger_services(
|
||||
node_interfaces::NodeServicesInterface::SharedPtr node_services) override;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(NodeLogging)
|
||||
|
||||
/// Handle to the NodeBaseInterface given in the constructor.
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
|
||||
rclcpp::node_interfaces::NodeBaseInterface * node_base_;
|
||||
|
||||
rclcpp::Logger logger_;
|
||||
|
||||
rclcpp::Service<rcl_interfaces::srv::GetLoggerLevels>::SharedPtr get_loggers_service_;
|
||||
rclcpp::Service<rcl_interfaces::srv::SetLoggerLevels>::SharedPtr set_loggers_service_;
|
||||
};
|
||||
|
||||
} // namespace node_interfaces
|
||||
|
||||
@@ -19,7 +19,6 @@
|
||||
|
||||
#include "rclcpp/logger.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/node_services_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -55,13 +54,6 @@ public:
|
||||
virtual
|
||||
const char *
|
||||
get_logger_name() const = 0;
|
||||
|
||||
/// create logger services
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
void
|
||||
create_logger_services(
|
||||
node_interfaces::NodeServicesInterface::SharedPtr node_services) = 0;
|
||||
};
|
||||
|
||||
} // namespace node_interfaces
|
||||
|
||||
@@ -1,63 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
|
||||
#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_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_topics_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace node_interfaces
|
||||
{
|
||||
|
||||
/// Implementation of the NodeTypeDescriptions part of the Node API.
|
||||
class NodeTypeDescriptions : public NodeTypeDescriptionsInterface
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptions)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
explicit NodeTypeDescriptions(
|
||||
NodeBaseInterface::SharedPtr node_base,
|
||||
NodeLoggingInterface::SharedPtr node_logging,
|
||||
NodeParametersInterface::SharedPtr node_parameters,
|
||||
NodeServicesInterface::SharedPtr node_services);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
~NodeTypeDescriptions();
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(NodeTypeDescriptions)
|
||||
|
||||
// Pimpl hides helper types and functions used for wrapping a C service, which would be
|
||||
// awkward to expose in this header.
|
||||
class NodeTypeDescriptionsImpl;
|
||||
std::unique_ptr<NodeTypeDescriptionsImpl> impl_;
|
||||
};
|
||||
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_
|
||||
@@ -1,44 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
|
||||
#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace node_interfaces
|
||||
{
|
||||
|
||||
/// Pure virtual interface class for the NodeTypeDescriptions part of the Node API.
|
||||
class NodeTypeDescriptionsInterface
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptionsInterface)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
~NodeTypeDescriptionsInterface() = default;
|
||||
};
|
||||
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(
|
||||
rclcpp::node_interfaces::NodeTypeDescriptionsInterface, type_descriptions)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_
|
||||
@@ -43,7 +43,6 @@ public:
|
||||
* - arguments = {}
|
||||
* - parameter_overrides = {}
|
||||
* - use_global_arguments = true
|
||||
* - enable_rosout = true
|
||||
* - use_intra_process_comms = false
|
||||
* - enable_topic_statistics = false
|
||||
* - start_parameter_services = true
|
||||
@@ -51,7 +50,6 @@ public:
|
||||
* - clock_type = RCL_ROS_TIME
|
||||
* - clock_qos = rclcpp::ClockQoS()
|
||||
* - use_clock_thread = true
|
||||
* - enable_logger_service = false
|
||||
* - rosout_qos = rclcpp::RosoutQoS()
|
||||
* - parameter_event_qos = rclcpp::ParameterEventQoS
|
||||
* - with history setting and depth from rmw_qos_profile_parameter_events
|
||||
@@ -234,24 +232,6 @@ public:
|
||||
NodeOptions &
|
||||
start_parameter_services(bool start_parameter_services);
|
||||
|
||||
/// Return the enable_logger_service flag.
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
enable_logger_service() const;
|
||||
|
||||
/// Set the enable_logger_service flag, return this for logger idiom.
|
||||
/**
|
||||
* If true, ROS services are created to allow external nodes to get
|
||||
* and set logger levels of this node.
|
||||
*
|
||||
* If false, loggers will still be configured and set logger levels locally,
|
||||
* but logger levels cannot be changed remotely .
|
||||
*
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
NodeOptions &
|
||||
enable_logger_service(bool enable_log_service);
|
||||
|
||||
/// Return the start_parameter_event_publisher flag.
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
@@ -441,8 +421,6 @@ private:
|
||||
|
||||
bool use_clock_thread_ {true};
|
||||
|
||||
bool enable_logger_service_ {false};
|
||||
|
||||
rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS(
|
||||
rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
|
||||
);
|
||||
|
||||
@@ -48,7 +48,7 @@ public:
|
||||
*
|
||||
* Example Usage:
|
||||
*
|
||||
* If you have received a parameter event and are only interested in parameters foo and
|
||||
* 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
|
||||
|
||||
@@ -24,7 +24,6 @@
|
||||
|
||||
#include "rcl_interfaces/msg/parameter_type.hpp"
|
||||
#include "rcl_interfaces/msg/parameter_value.hpp"
|
||||
#include "rclcpp/exceptions/exceptions.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
|
||||
@@ -32,9 +32,6 @@
|
||||
#include "rclcpp/allocator/allocator_common.hpp"
|
||||
#include "rclcpp/allocator/allocator_deleter.hpp"
|
||||
#include "rclcpp/detail/resolve_use_intra_process.hpp"
|
||||
#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp"
|
||||
#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
|
||||
#include "rclcpp/experimental/create_intra_process_buffer.hpp"
|
||||
#include "rclcpp/experimental/intra_process_manager.hpp"
|
||||
#include "rclcpp/get_message_type_support_handle.hpp"
|
||||
#include "rclcpp/is_ros_compatible_type.hpp"
|
||||
@@ -112,12 +109,6 @@ public:
|
||||
[[deprecated("use std::shared_ptr<const PublishedType>")]] =
|
||||
std::shared_ptr<const PublishedType>;
|
||||
|
||||
using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
|
||||
ROSMessageType,
|
||||
ROSMessageTypeAllocator,
|
||||
ROSMessageTypeDeleter
|
||||
>::SharedPtr;
|
||||
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, AllocatorT>)
|
||||
|
||||
/// Default constructor.
|
||||
@@ -180,14 +171,11 @@ public:
|
||||
throw std::invalid_argument(
|
||||
"intraprocess communication is not allowed with a zero qos history depth value");
|
||||
}
|
||||
if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
|
||||
buffer_ = rclcpp::experimental::create_intra_process_buffer<
|
||||
ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter>(
|
||||
rclcpp::detail::resolve_intra_process_buffer_type(options_.intra_process_buffer_type),
|
||||
qos,
|
||||
std::make_shared<ROSMessageTypeAllocator>(ros_message_type_allocator_));
|
||||
if (qos.durability() != rclcpp::DurabilityPolicy::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(), buffer_);
|
||||
uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
|
||||
this->setup_intra_process(
|
||||
intra_process_publisher_id,
|
||||
ipm);
|
||||
@@ -254,18 +242,9 @@ public:
|
||||
if (inter_process_publish_needed) {
|
||||
auto shared_msg =
|
||||
this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg));
|
||||
if (buffer_) {
|
||||
buffer_->add_shared(shared_msg);
|
||||
}
|
||||
this->do_inter_process_publish(*shared_msg);
|
||||
} else {
|
||||
if (buffer_) {
|
||||
auto shared_msg =
|
||||
this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg));
|
||||
buffer_->add_shared(shared_msg);
|
||||
} else {
|
||||
this->do_intra_process_ros_message_publish(std::move(msg));
|
||||
}
|
||||
this->do_intra_process_ros_message_publish(std::move(msg));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -290,8 +269,8 @@ public:
|
||||
{
|
||||
// Avoid allocating when not using intra process.
|
||||
if (!intra_process_is_enabled_) {
|
||||
this->do_inter_process_publish(msg);
|
||||
return;
|
||||
// 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.
|
||||
@@ -318,31 +297,26 @@ public:
|
||||
>
|
||||
publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
|
||||
{
|
||||
// Avoid allocating when not using intra process.
|
||||
if (!intra_process_is_enabled_) {
|
||||
// In this case we're not using intra process.
|
||||
auto ros_msg_ptr = std::make_unique<ROSMessageType>();
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
|
||||
this->do_inter_process_publish(*ros_msg_ptr);
|
||||
return;
|
||||
ROSMessageType ros_msg;
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
|
||||
return this->do_inter_process_publish(ros_msg);
|
||||
}
|
||||
|
||||
bool inter_process_publish_needed =
|
||||
get_subscription_count() > get_intra_process_subscription_count();
|
||||
|
||||
if (inter_process_publish_needed) {
|
||||
auto ros_msg_ptr = std::make_shared<ROSMessageType>();
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
|
||||
ROSMessageType ros_msg;
|
||||
// TODO(clalancette): This is unnecessarily doing an additional conversion
|
||||
// that may have already been done in do_intra_process_publish_and_return_shared().
|
||||
// We should just reuse that effort.
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, ros_msg);
|
||||
this->do_intra_process_publish(std::move(msg));
|
||||
this->do_inter_process_publish(*ros_msg_ptr);
|
||||
if (buffer_) {
|
||||
buffer_->add_shared(ros_msg_ptr);
|
||||
}
|
||||
this->do_inter_process_publish(ros_msg);
|
||||
} else {
|
||||
if (buffer_) {
|
||||
auto ros_msg_ptr = std::make_shared<ROSMessageType>();
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
|
||||
buffer_->add_shared(ros_msg_ptr);
|
||||
}
|
||||
this->do_intra_process_publish(std::move(msg));
|
||||
}
|
||||
}
|
||||
@@ -365,12 +339,13 @@ public:
|
||||
>
|
||||
publish(const T & msg)
|
||||
{
|
||||
// Avoid double allocating when not using intra process.
|
||||
if (!intra_process_is_enabled_) {
|
||||
// Convert to the ROS message equivalent and publish it.
|
||||
auto ros_msg_ptr = std::make_unique<ROSMessageType>();
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ros_msg_ptr);
|
||||
this->do_inter_process_publish(*ros_msg_ptr);
|
||||
return;
|
||||
ROSMessageType ros_msg;
|
||||
rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, ros_msg);
|
||||
// In this case we're not using intra process.
|
||||
return this->do_inter_process_publish(ros_msg);
|
||||
}
|
||||
|
||||
// Otherwise we have to allocate memory in a unique_ptr and pass it along.
|
||||
@@ -415,7 +390,7 @@ public:
|
||||
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());
|
||||
this->do_loaned_message_publish(std::move(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.
|
||||
@@ -446,7 +421,7 @@ protected:
|
||||
void
|
||||
do_inter_process_publish(const ROSMessageType & msg)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
|
||||
TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
|
||||
auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);
|
||||
|
||||
if (RCL_RET_PUBLISHER_INVALID == status) {
|
||||
@@ -481,7 +456,6 @@ protected:
|
||||
do_loaned_message_publish(
|
||||
std::unique_ptr<ROSMessageType, std::function<void(ROSMessageType *)>> msg)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(msg.get()));
|
||||
auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr);
|
||||
|
||||
if (RCL_RET_PUBLISHER_INVALID == status) {
|
||||
@@ -510,10 +484,6 @@ protected:
|
||||
if (!msg) {
|
||||
throw std::runtime_error("cannot publish msg which is a null pointer");
|
||||
}
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_intra_publish,
|
||||
static_cast<const void *>(publisher_handle_.get()),
|
||||
msg.get());
|
||||
|
||||
ipm->template do_intra_process_publish<PublishedType, ROSMessageType, AllocatorT>(
|
||||
intra_process_publisher_id_,
|
||||
@@ -532,10 +502,6 @@ protected:
|
||||
if (!msg) {
|
||||
throw std::runtime_error("cannot publish msg which is a null pointer");
|
||||
}
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_intra_publish,
|
||||
static_cast<const void *>(publisher_handle_.get()),
|
||||
msg.get());
|
||||
|
||||
ipm->template do_intra_process_publish<ROSMessageType, ROSMessageType, AllocatorT>(
|
||||
intra_process_publisher_id_,
|
||||
@@ -555,10 +521,6 @@ protected:
|
||||
if (!msg) {
|
||||
throw std::runtime_error("cannot publish msg which is a null pointer");
|
||||
}
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
rclcpp_intra_publish,
|
||||
static_cast<const void *>(publisher_handle_.get()),
|
||||
msg.get());
|
||||
|
||||
return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType, ROSMessageType,
|
||||
AllocatorT>(
|
||||
@@ -606,8 +568,6 @@ protected:
|
||||
PublishedTypeDeleter published_type_deleter_;
|
||||
ROSMessageTypeAllocator ros_message_type_allocator_;
|
||||
ROSMessageTypeDeleter ros_message_type_deleter_;
|
||||
|
||||
BufferSharedPtr buffer_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -139,12 +139,6 @@ public:
|
||||
size_t
|
||||
get_intra_process_subscription_count() const;
|
||||
|
||||
/// Get if durability is transient local
|
||||
/** \return If durability is transient local*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_durability_transient_local() 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
|
||||
@@ -221,17 +215,6 @@ public:
|
||||
std::vector<rclcpp::NetworkFlowEndpoint>
|
||||
get_network_flow_endpoints() const;
|
||||
|
||||
/// Return the lowest available capacity for all subscription buffers.
|
||||
/**
|
||||
* For intraprocess communication return the lowest buffer capacity for all subscriptions.
|
||||
* If intraprocess is disabled or no intraprocess subscriptions present, return maximum of size_t.
|
||||
* On failure return 0.
|
||||
* \return lowest buffer capacity for all subscriptions
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
lowest_available_ipm_capacity() const;
|
||||
|
||||
/// Wait until all published messages are acknowledged or until the specified timeout elapses.
|
||||
/**
|
||||
* This method waits until all published messages are acknowledged by all matching
|
||||
|
||||
@@ -24,7 +24,6 @@
|
||||
|
||||
#include "rclcpp/allocator/allocator_common.hpp"
|
||||
#include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp"
|
||||
#include "rclcpp/intra_process_buffer_type.hpp"
|
||||
#include "rclcpp/intra_process_setting.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/event_handler.hpp"
|
||||
@@ -41,9 +40,6 @@ struct PublisherOptionsBase
|
||||
/// 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::SharedPtr;
|
||||
|
||||
/// Callbacks for various events related to publishers.
|
||||
PublisherEventCallbacks event_callbacks;
|
||||
|
||||
|
||||
22
rclcpp/include/rclcpp/qos_event.hpp
Normal file
22
rclcpp/include/rclcpp/qos_event.hpp
Normal file
@@ -0,0 +1,22 @@
|
||||
// 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_
|
||||
|
||||
#warning This header is obsolete, please include rclcpp/event_handler.hpp instead
|
||||
|
||||
#include "rclcpp/event_handler.hpp"
|
||||
|
||||
#endif // RCLCPP__QOS_EVENT_HPP_
|
||||
@@ -19,8 +19,6 @@
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/clock.hpp"
|
||||
#include "rclcpp/duration.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/utilities.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
@@ -33,20 +31,9 @@ class RateBase
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~RateBase() {}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual bool sleep() = 0;
|
||||
|
||||
[[deprecated("use get_type() instead")]]
|
||||
RCLCPP_PUBLIC
|
||||
virtual bool is_steady() const = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual rcl_clock_type_t get_type() const = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual void reset() = 0;
|
||||
};
|
||||
|
||||
@@ -55,13 +42,14 @@ using std::chrono::duration_cast;
|
||||
using std::chrono::nanoseconds;
|
||||
|
||||
template<class Clock = std::chrono::high_resolution_clock>
|
||||
class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase
|
||||
class GenericRate : public RateBase
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
|
||||
|
||||
explicit GenericRate(double rate)
|
||||
: period_(duration_cast<nanoseconds>(duration<double>(1.0 / rate))), last_interval_(Clock::now())
|
||||
: GenericRate<Clock>(
|
||||
duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
|
||||
{}
|
||||
explicit GenericRate(std::chrono::nanoseconds period)
|
||||
: period_(period), last_interval_(Clock::now())
|
||||
@@ -99,18 +87,12 @@ public:
|
||||
return true;
|
||||
}
|
||||
|
||||
[[deprecated("use get_type() instead")]]
|
||||
virtual bool
|
||||
is_steady() const
|
||||
{
|
||||
return Clock::is_steady;
|
||||
}
|
||||
|
||||
virtual rcl_clock_type_t get_type() const
|
||||
{
|
||||
return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME;
|
||||
}
|
||||
|
||||
virtual void
|
||||
reset()
|
||||
{
|
||||
@@ -130,59 +112,8 @@ private:
|
||||
std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
|
||||
};
|
||||
|
||||
class Rate : public RateBase
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(Rate)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
explicit Rate(
|
||||
const double rate,
|
||||
Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
explicit Rate(
|
||||
const Duration & period,
|
||||
Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual bool
|
||||
sleep();
|
||||
|
||||
[[deprecated("use get_type() instead")]]
|
||||
RCLCPP_PUBLIC
|
||||
virtual bool
|
||||
is_steady() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual rcl_clock_type_t
|
||||
get_type() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual void
|
||||
reset();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::chrono::nanoseconds
|
||||
period() const;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(Rate)
|
||||
|
||||
Clock::SharedPtr clock_;
|
||||
Duration period_;
|
||||
Time last_interval_;
|
||||
};
|
||||
|
||||
class WallRate : public Rate
|
||||
{
|
||||
public:
|
||||
RCLCPP_PUBLIC
|
||||
explicit WallRate(const double rate);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
explicit WallRate(const Duration & period);
|
||||
};
|
||||
using Rate = GenericRate<std::chrono::system_clock>;
|
||||
using WallRate = GenericRate<std::chrono::steady_clock>;
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
@@ -54,7 +54,6 @@
|
||||
* - rclcpp::ParameterValue
|
||||
* - rclcpp::AsyncParametersClient
|
||||
* - rclcpp::SyncParametersClient
|
||||
* - rclcpp::copy_all_parameter_values()
|
||||
* - rclcpp/parameter.hpp
|
||||
* - rclcpp/parameter_value.hpp
|
||||
* - rclcpp/parameter_client.hpp
|
||||
@@ -96,9 +95,6 @@
|
||||
* - Get the number of publishers or subscribers on a topic:
|
||||
* - rclcpp::Node::count_publishers()
|
||||
* - rclcpp::Node::count_subscribers()
|
||||
* - Get the number of clients or servers on a service:
|
||||
* - rclcpp::Node::count_clients()
|
||||
* - rclcpp::Node::count_services()
|
||||
*
|
||||
* And components related to logging:
|
||||
*
|
||||
@@ -133,6 +129,9 @@
|
||||
* - Dynamic typesupport
|
||||
* - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport
|
||||
* - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp
|
||||
* - Dynamic subscription
|
||||
* - rclcpp::DynamicSubscription
|
||||
* - rclcpp/dynamic_subscription.hpp
|
||||
* - Generic publisher
|
||||
* - rclcpp::Node::create_generic_publisher()
|
||||
* - rclcpp::GenericPublisher
|
||||
@@ -168,7 +167,6 @@
|
||||
#include <csignal>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/copy_all_parameter_values.hpp"
|
||||
#include "rclcpp/executors.hpp"
|
||||
#include "rclcpp/guard_condition.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
@@ -184,4 +182,6 @@
|
||||
#include "rclcpp/waitable.hpp"
|
||||
#include "rclcpp/wait_set.hpp"
|
||||
|
||||
#include "rclcpp/dynamic_subscription.hpp"
|
||||
|
||||
#endif // RCLCPP__RCLCPP_HPP_
|
||||
|
||||
@@ -265,19 +265,15 @@ protected:
|
||||
|
||||
std::shared_ptr<rcl_node_t> node_handle_;
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
// It is important to declare on_new_request_callback_ before
|
||||
// service_handle_, so on destruction the service is
|
||||
// destroyed first. Otherwise, the rmw service callback
|
||||
// would point briefly to a destroyed function.
|
||||
std::function<void(size_t)> on_new_request_callback_{nullptr};
|
||||
// Declare service_handle_ after callback
|
||||
std::shared_ptr<rcl_service_t> service_handle_;
|
||||
bool owns_rcl_handle_ = true;
|
||||
|
||||
rclcpp::Logger node_logger_;
|
||||
|
||||
std::atomic<bool> in_use_by_wait_set_{false};
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
std::function<void(size_t)> on_new_request_callback_{nullptr};
|
||||
};
|
||||
|
||||
template<typename ServiceT>
|
||||
@@ -352,7 +348,7 @@ public:
|
||||
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
|
||||
}
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_service_callback_added,
|
||||
static_cast<const void *>(get_service_handle().get()),
|
||||
static_cast<const void *>(&any_callback_));
|
||||
@@ -387,7 +383,7 @@ public:
|
||||
}
|
||||
|
||||
service_handle_ = service_handle;
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_service_callback_added,
|
||||
static_cast<const void *>(get_service_handle().get()),
|
||||
static_cast<const void *>(&any_callback_));
|
||||
@@ -424,7 +420,7 @@ public:
|
||||
// In this case, rcl owns the service handle memory
|
||||
service_handle_ = std::shared_ptr<rcl_service_t>(new rcl_service_t);
|
||||
service_handle_->impl = service_handle->impl;
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_service_callback_added,
|
||||
static_cast<const void *>(get_service_handle().get()),
|
||||
static_cast<const void *>(&any_callback_));
|
||||
@@ -486,14 +482,6 @@ public:
|
||||
{
|
||||
rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response);
|
||||
|
||||
if (ret == RCL_RET_TIMEOUT) {
|
||||
RCLCPP_WARN(
|
||||
node_logger_.get_child("rclcpp"),
|
||||
"failed to send response to %s (timeout): %s",
|
||||
this->get_service_name(), rcl_get_error_string().str);
|
||||
rcl_reset_error();
|
||||
return;
|
||||
}
|
||||
if (ret != RCL_RET_OK) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response");
|
||||
}
|
||||
|
||||
@@ -15,17 +15,10 @@
|
||||
#ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
|
||||
#define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
#include <type_traits>
|
||||
|
||||
#include "rosidl_runtime_cpp/traits.hpp"
|
||||
|
||||
#include "rclcpp/logger.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/message_memory_strategy.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
@@ -57,24 +50,13 @@ class MessagePoolMemoryStrategy
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy)
|
||||
|
||||
/// Default constructor
|
||||
MessagePoolMemoryStrategy()
|
||||
: next_array_index_(0)
|
||||
{
|
||||
pool_mutex_ = std::make_shared<std::mutex>();
|
||||
|
||||
pool_ = std::shared_ptr<std::array<MessageT *, Size>>(
|
||||
new std::array<MessageT *, Size>,
|
||||
[](std::array<MessageT *, Size> * arr) {
|
||||
for (size_t i = 0; i < Size; ++i) {
|
||||
free((*arr)[i]);
|
||||
}
|
||||
delete arr;
|
||||
});
|
||||
|
||||
free_list_ = std::make_shared<CircularArray<Size>>();
|
||||
|
||||
for (size_t i = 0; i < Size; ++i) {
|
||||
(*pool_)[i] = static_cast<MessageT *>(malloc(sizeof(MessageT)));
|
||||
free_list_->push_back(i);
|
||||
pool_[i].msg_ptr_ = std::make_shared<MessageT>();
|
||||
pool_[i].used = false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -86,85 +68,43 @@ public:
|
||||
*/
|
||||
std::shared_ptr<MessageT> borrow_message()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(*pool_mutex_);
|
||||
if (free_list_->size() == 0) {
|
||||
throw std::runtime_error("No more free slots in the pool");
|
||||
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;
|
||||
|
||||
size_t current_index = free_list_->pop_front();
|
||||
|
||||
return std::shared_ptr<MessageT>(
|
||||
new((*pool_)[current_index]) MessageT(),
|
||||
[pool = this->pool_, pool_mutex = this->pool_mutex_,
|
||||
free_list = this->free_list_](MessageT * p) {
|
||||
std::lock_guard<std::mutex> lock(*pool_mutex);
|
||||
for (size_t i = 0; i < Size; ++i) {
|
||||
if ((*pool)[i] == p) {
|
||||
p->~MessageT();
|
||||
free_list->push_back(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
});
|
||||
pool_[current_index].used = true;
|
||||
return pool_[current_index].msg_ptr_;
|
||||
}
|
||||
|
||||
/// Return a message to the message pool.
|
||||
/**
|
||||
* This does nothing since the message isn't returned to the pool until the user has dropped
|
||||
* all references.
|
||||
* 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<MessageT> & msg)
|
||||
{
|
||||
(void)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:
|
||||
template<size_t N>
|
||||
class CircularArray
|
||||
struct PoolMember
|
||||
{
|
||||
public:
|
||||
void push_back(const size_t v)
|
||||
{
|
||||
if (size_ + 1 > N) {
|
||||
throw std::runtime_error("Tried to push too many items into the array");
|
||||
}
|
||||
array_[(front_ + size_) % N] = v;
|
||||
++size_;
|
||||
}
|
||||
|
||||
size_t pop_front()
|
||||
{
|
||||
if (size_ < 1) {
|
||||
throw std::runtime_error("Tried to pop item from empty array");
|
||||
}
|
||||
|
||||
size_t val = array_[front_];
|
||||
|
||||
front_ = (front_ + 1) % N;
|
||||
--size_;
|
||||
|
||||
return val;
|
||||
}
|
||||
|
||||
size_t size() const
|
||||
{
|
||||
return size_;
|
||||
}
|
||||
|
||||
private:
|
||||
size_t front_ = 0;
|
||||
size_t size_ = 0;
|
||||
std::array<size_t, N> array_;
|
||||
std::shared_ptr<MessageT> msg_ptr_;
|
||||
bool used;
|
||||
};
|
||||
|
||||
// It's very important that these are shared_ptrs, since users of this class might hold a
|
||||
// reference to a pool item longer than the lifetime of the class. In that scenario, the
|
||||
// shared_ptr ensures that the lifetime of these variables outlives this class, and hence ensures
|
||||
// the custom destructor for each pool item can successfully run.
|
||||
std::shared_ptr<std::mutex> pool_mutex_;
|
||||
std::shared_ptr<std::array<MessageT *, Size>> pool_;
|
||||
std::shared_ptr<CircularArray<Size>> free_list_;
|
||||
std::array<PoolMember, Size> pool_;
|
||||
size_t next_array_index_;
|
||||
};
|
||||
|
||||
} // namespace message_pool_memory_strategy
|
||||
|
||||
@@ -104,7 +104,7 @@ public:
|
||||
|
||||
private:
|
||||
using SubscriptionTopicStatisticsSharedPtr =
|
||||
std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>;
|
||||
std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>;
|
||||
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
|
||||
@@ -127,7 +127,6 @@ public:
|
||||
* of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL,
|
||||
* qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE).
|
||||
*/
|
||||
// *INDENT-OFF*
|
||||
Subscription(
|
||||
rclcpp::node_interfaces::NodeBaseInterface * node_base,
|
||||
const rosidl_message_type_support_t & type_support_handle,
|
||||
@@ -145,11 +144,10 @@ public:
|
||||
// NOTE(methylDragon): Passing these args separately is necessary for event binding
|
||||
options.event_callbacks,
|
||||
options.use_default_callbacks,
|
||||
callback.is_serialized_message_callback() ? DeliveredMessageKind::SERIALIZED_MESSAGE : DeliveredMessageKind::ROS_MESSAGE), // NOLINT
|
||||
callback.is_serialized_message_callback() ? SubscriptionType::SERIALIZED_MESSAGE : SubscriptionType::ROS_MESSAGE), // NOLINT
|
||||
any_callback_(callback),
|
||||
options_(options),
|
||||
message_memory_strategy_(message_memory_strategy)
|
||||
// *INDENT-ON*
|
||||
{
|
||||
// Setup intra process publishing if requested.
|
||||
if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
|
||||
@@ -165,6 +163,10 @@ public:
|
||||
throw std::invalid_argument(
|
||||
"intraprocess communication is not allowed with 0 depth qos policy");
|
||||
}
|
||||
if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
|
||||
throw std::invalid_argument(
|
||||
"intraprocess communication allowed only with volatile durability");
|
||||
}
|
||||
|
||||
using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
|
||||
MessageT,
|
||||
@@ -183,7 +185,7 @@ public:
|
||||
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));
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_subscription_init,
|
||||
static_cast<const void *>(get_subscription_handle().get()),
|
||||
static_cast<const void *>(subscription_intra_process_.get()));
|
||||
@@ -191,8 +193,7 @@ public:
|
||||
// Add it to the intra process manager.
|
||||
using rclcpp::experimental::IntraProcessManager;
|
||||
auto ipm = context->get_sub_context<IntraProcessManager>();
|
||||
uint64_t intra_process_subscription_id = ipm->template add_subscription<
|
||||
ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_);
|
||||
uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process_);
|
||||
this->setup_intra_process(intra_process_subscription_id, ipm);
|
||||
}
|
||||
|
||||
@@ -200,11 +201,11 @@ public:
|
||||
this->subscription_topic_statistics_ = std::move(subscription_topic_statistics);
|
||||
}
|
||||
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_subscription_init,
|
||||
static_cast<const void *>(get_subscription_handle().get()),
|
||||
static_cast<const void *>(this));
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_subscription_callback_added,
|
||||
static_cast<const void *>(this),
|
||||
static_cast<const void *>(&any_callback_));
|
||||
@@ -315,7 +316,7 @@ public:
|
||||
if (subscription_topic_statistics_) {
|
||||
const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
|
||||
const auto time = rclcpp::Time(nanos.time_since_epoch().count());
|
||||
subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
|
||||
subscription_topic_statistics_->handle_message(*typed_message, time);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -324,20 +325,8 @@ public:
|
||||
const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
|
||||
const rclcpp::MessageInfo & message_info) override
|
||||
{
|
||||
std::chrono::time_point<std::chrono::system_clock> now;
|
||||
if (subscription_topic_statistics_) {
|
||||
// get current time before executing callback to
|
||||
// exclude callback duration from topic statistics result.
|
||||
now = std::chrono::system_clock::now();
|
||||
}
|
||||
|
||||
// TODO(wjwwood): enable topic statistics for serialized messages
|
||||
any_callback_.dispatch(serialized_message, message_info);
|
||||
|
||||
if (subscription_topic_statistics_) {
|
||||
const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
|
||||
const auto time = rclcpp::Time(nanos.time_since_epoch().count());
|
||||
subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
@@ -368,7 +357,7 @@ public:
|
||||
if (subscription_topic_statistics_) {
|
||||
const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
|
||||
const auto time = rclcpp::Time(nanos.time_since_epoch().count());
|
||||
subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
|
||||
subscription_topic_statistics_->handle_message(*typed_message, time);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -63,25 +63,12 @@ namespace experimental
|
||||
class IntraProcessManager;
|
||||
} // namespace experimental
|
||||
|
||||
/// The kind of message that the subscription delivers in its callback, used by the executor
|
||||
/**
|
||||
* This enum needs to exist because the callback handle is not accessible to the executor's scope.
|
||||
*
|
||||
* "Kind" is used since what is being delivered is a category of messages, for example, there are
|
||||
* different ROS message types that can be delivered, but they're all ROS messages.
|
||||
*
|
||||
* As a concrete example, all of the following callbacks will be considered ROS_MESSAGE for
|
||||
* DeliveredMessageKind:
|
||||
* - void callback(const std_msgs::msg::String &)
|
||||
* - void callback(const std::string &) // type adaption
|
||||
* - void callback(std::unique_ptr<std_msgs::msg::String>)
|
||||
*/
|
||||
enum class DeliveredMessageKind : uint8_t
|
||||
enum class SubscriptionType : uint8_t
|
||||
{
|
||||
INVALID = 0,
|
||||
ROS_MESSAGE = 1, // The subscription delivers a ROS message to its callback
|
||||
SERIALIZED_MESSAGE = 2, // The subscription delivers a serialized message to its callback
|
||||
DYNAMIC_MESSAGE = 3, // The subscription delivers a dynamic message to its callback
|
||||
ROS_MESSAGE = 1, // take message as ROS message and handle as ROS message
|
||||
SERIALIZED_MESSAGE = 2, // take message as serialized and handle as serialized
|
||||
DYNAMIC_MESSAGE_DIRECT = 3, // take message as DynamicMessage and handle as DynamicMessage
|
||||
DYNAMIC_MESSAGE_FROM_SERIALIZED = 4 // take message as serialized and handle as DynamicMessage
|
||||
};
|
||||
|
||||
/// Virtual base class for subscriptions. This pattern allows us to iterate over different template
|
||||
@@ -100,8 +87,7 @@ public:
|
||||
* \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] delivered_message_kind Enum flag to change how the message will be received and
|
||||
* delivered
|
||||
* \param[in] subscription_type Enum flag to change how the message will be received and delivered
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
SubscriptionBase(
|
||||
@@ -111,7 +97,7 @@ public:
|
||||
const rcl_subscription_options_t & subscription_options,
|
||||
const SubscriptionEventCallbacks & event_callbacks,
|
||||
bool use_default_callbacks,
|
||||
DeliveredMessageKind delivered_message_kind = DeliveredMessageKind::ROS_MESSAGE);
|
||||
SubscriptionType subscription_type = SubscriptionType::ROS_MESSAGE);
|
||||
|
||||
/// Destructor.
|
||||
RCLCPP_PUBLIC
|
||||
@@ -252,21 +238,13 @@ public:
|
||||
const rosidl_message_type_support_t &
|
||||
get_message_type_support_handle() const;
|
||||
|
||||
/// Return if the subscription is serialized
|
||||
/// Return the type of the subscription.
|
||||
/**
|
||||
* \return `true` if the subscription is serialized, `false` otherwise
|
||||
* \return `SubscriptionType`, which adjusts how messages are received and delivered.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_serialized() const;
|
||||
|
||||
/// Return the delivered message kind.
|
||||
/**
|
||||
* \return `DeliveredMessageKind`, which adjusts how messages are received and delivered.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
DeliveredMessageKind
|
||||
get_delivered_message_kind() const;
|
||||
SubscriptionType
|
||||
get_subscription_type() const;
|
||||
|
||||
/// Get matching publisher count.
|
||||
/** \return The number of publishers on this topic. */
|
||||
@@ -644,15 +622,11 @@ protected:
|
||||
|
||||
rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
|
||||
|
||||
std::shared_ptr<rcl_node_t> node_handle_;
|
||||
// TODO(methylDragon): Remove if we don't need this
|
||||
// rclcpp::node_interfaces::NodeGraphInterface * const node_graph_;
|
||||
// rclcpp::node_interfaces::NodeServicesInterface * const node_services_;
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
// It is important to declare on_new_message_callback_ before
|
||||
// subscription_handle_, so on destruction the subscription is
|
||||
// destroyed first. Otherwise, the rmw subscription callback
|
||||
// would point briefly to a destroyed function.
|
||||
std::function<void(size_t)> on_new_message_callback_{nullptr};
|
||||
// Declare subscription_handle_ after callback
|
||||
std::shared_ptr<rcl_node_t> node_handle_;
|
||||
std::shared_ptr<rcl_subscription_t> subscription_handle_;
|
||||
std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
|
||||
rclcpp::Logger node_logger_;
|
||||
@@ -671,12 +645,15 @@ private:
|
||||
RCLCPP_DISABLE_COPY(SubscriptionBase)
|
||||
|
||||
rosidl_message_type_support_t type_support_;
|
||||
DeliveredMessageKind delivered_message_kind_;
|
||||
SubscriptionType subscription_type_;
|
||||
|
||||
std::atomic<bool> subscription_in_use_by_wait_set_{false};
|
||||
std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
|
||||
std::unordered_map<rclcpp::EventHandlerBase *,
|
||||
std::atomic<bool>> qos_events_in_use_by_wait_set_;
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
std::function<void(size_t)> on_new_message_callback_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -75,14 +75,15 @@ template<
|
||||
typename CallbackT,
|
||||
typename AllocatorT,
|
||||
typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
|
||||
typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType
|
||||
typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType,
|
||||
typename ROSMessageType = typename SubscriptionT::ROSMessageType
|
||||
>
|
||||
SubscriptionFactory
|
||||
create_subscription_factory(
|
||||
CallbackT && callback,
|
||||
const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
|
||||
typename MessageMemoryStrategyT::SharedPtr msg_mem_strat,
|
||||
std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
|
||||
std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<ROSMessageType>>
|
||||
subscription_topic_stats = nullptr
|
||||
)
|
||||
{
|
||||
|
||||
@@ -77,10 +77,6 @@ struct SubscriptionOptionsBase
|
||||
// Topic statistics publication period in ms. Defaults to one second.
|
||||
// Only values greater than zero are allowed.
|
||||
std::chrono::milliseconds publish_period{std::chrono::seconds(1)};
|
||||
|
||||
// An optional QoS which can provide topic_statistics with a stable QoS separate from
|
||||
// the subscription's current QoS settings which could be unstable.
|
||||
rclcpp::QoS qos = SystemDefaultsQoS();
|
||||
};
|
||||
|
||||
TopicStatisticsOptions topic_stats_options;
|
||||
|
||||
@@ -57,10 +57,6 @@ public:
|
||||
RCLCPP_PUBLIC
|
||||
Time(const Time & rhs);
|
||||
|
||||
/// Move constructor
|
||||
RCLCPP_PUBLIC
|
||||
Time(Time && rhs) noexcept;
|
||||
|
||||
/// Time constructor
|
||||
/**
|
||||
* \param time_msg builtin_interfaces time message to copy
|
||||
@@ -88,7 +84,6 @@ public:
|
||||
operator builtin_interfaces::msg::Time() const;
|
||||
|
||||
/**
|
||||
* Copy assignment operator
|
||||
* \throws std::runtime_error if seconds are negative
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
@@ -105,13 +100,6 @@ public:
|
||||
Time &
|
||||
operator=(const builtin_interfaces::msg::Time & time_msg);
|
||||
|
||||
/**
|
||||
* Move assignment operator
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
Time &
|
||||
operator=(Time && rhs) noexcept;
|
||||
|
||||
/**
|
||||
* \throws std::runtime_error if the time sources are different
|
||||
*/
|
||||
@@ -201,7 +189,7 @@ public:
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
static Time
|
||||
max(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); // NOLINT
|
||||
max();
|
||||
|
||||
/// Get the seconds since epoch
|
||||
/**
|
||||
@@ -234,15 +222,6 @@ RCLCPP_PUBLIC
|
||||
Time
|
||||
operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
|
||||
|
||||
/// Convert rcl_time_point_value_t to builtin_interfaces::msg::Time
|
||||
/**
|
||||
* \param[in] time_point is a rcl_time_point_value_t
|
||||
* \return the builtin_interfaces::msg::Time from the time_point
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
builtin_interfaces::msg::Time
|
||||
convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point);
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__TIME_HPP_
|
||||
|
||||
@@ -53,17 +53,12 @@ public:
|
||||
* \param clock A clock to use for time and sleeping
|
||||
* \param period The interval at which the timer fires
|
||||
* \param context node context
|
||||
* \param autostart timer state on initialization
|
||||
*
|
||||
* In order to activate a timer that is not started on initialization,
|
||||
* user should call the reset() method.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit TimerBase(
|
||||
Clock::SharedPtr clock,
|
||||
std::chrono::nanoseconds period,
|
||||
rclcpp::Context::SharedPtr context,
|
||||
bool autostart = true);
|
||||
rclcpp::Context::SharedPtr context);
|
||||
|
||||
/// TimerBase destructor
|
||||
RCLCPP_PUBLIC
|
||||
@@ -221,22 +216,21 @@ public:
|
||||
* \param[in] period The interval at which the timer fires.
|
||||
* \param[in] callback User-specified callback function.
|
||||
* \param[in] context custom context to be used.
|
||||
* \param autostart timer state on initialization
|
||||
*/
|
||||
explicit GenericTimer(
|
||||
Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
|
||||
rclcpp::Context::SharedPtr context, bool autostart = true
|
||||
rclcpp::Context::SharedPtr context
|
||||
)
|
||||
: TimerBase(clock, period, context, autostart), callback_(std::forward<FunctorT>(callback))
|
||||
: TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_timer_callback_added,
|
||||
static_cast<const void *>(get_timer_handle().get()),
|
||||
reinterpret_cast<const void *>(&callback_));
|
||||
#ifndef TRACETOOLS_DISABLED
|
||||
if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
char * symbol = tracetools::get_symbol(callback_);
|
||||
TRACETOOLS_DO_TRACEPOINT(
|
||||
DO_TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
reinterpret_cast<const void *>(&callback_),
|
||||
symbol);
|
||||
@@ -275,9 +269,9 @@ public:
|
||||
void
|
||||
execute_callback() override
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
|
||||
TRACEPOINT(callback_start, reinterpret_cast<const void *>(&callback_), false);
|
||||
execute_callback_delegate<>();
|
||||
TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
|
||||
TRACEPOINT(callback_end, reinterpret_cast<const void *>(&callback_));
|
||||
}
|
||||
|
||||
// void specialization
|
||||
@@ -336,15 +330,13 @@ public:
|
||||
* \param period The interval at which the timer fires
|
||||
* \param callback The callback function to execute every interval
|
||||
* \param context node context
|
||||
* \param autostart timer state on initialization
|
||||
*/
|
||||
WallTimer(
|
||||
std::chrono::nanoseconds period,
|
||||
FunctorT && callback,
|
||||
rclcpp::Context::SharedPtr context,
|
||||
bool autostart = true)
|
||||
rclcpp::Context::SharedPtr context)
|
||||
: GenericTimer<FunctorT>(
|
||||
std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context, autostart)
|
||||
std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context)
|
||||
{}
|
||||
|
||||
protected:
|
||||
|
||||
@@ -48,12 +48,21 @@ using libstatistics_collector::moving_average_statistics::StatisticData;
|
||||
/**
|
||||
* Class used to collect, measure, and publish topic statistics data. Current statistics
|
||||
* supported for subscribers are received message age and received message period.
|
||||
*/
|
||||
*
|
||||
* \tparam CallbackMessageT the subscribed message type
|
||||
*/
|
||||
template<typename CallbackMessageT>
|
||||
class SubscriptionTopicStatistics
|
||||
{
|
||||
using TopicStatsCollector = libstatistics_collector::TopicStatisticsCollector;
|
||||
using ReceivedMessageAge = libstatistics_collector::ReceivedMessageAgeCollector;
|
||||
using ReceivedMessagePeriod = libstatistics_collector::ReceivedMessagePeriodCollector;
|
||||
using TopicStatsCollector =
|
||||
libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector<
|
||||
CallbackMessageT>;
|
||||
using ReceivedMessageAge =
|
||||
libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector<
|
||||
CallbackMessageT>;
|
||||
using ReceivedMessagePeriod =
|
||||
libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector<
|
||||
CallbackMessageT>;
|
||||
|
||||
public:
|
||||
/// Construct a SubscriptionTopicStatistics object.
|
||||
@@ -92,16 +101,16 @@ public:
|
||||
/**
|
||||
* This method acquires a lock to prevent race conditions to collectors list.
|
||||
*
|
||||
* \param message_info the message info corresponding to the received message
|
||||
* \param received_message the message received by the subscription
|
||||
* \param now_nanoseconds current time in nanoseconds
|
||||
*/
|
||||
virtual void handle_message(
|
||||
const rmw_message_info_t & message_info,
|
||||
const CallbackMessageT & received_message,
|
||||
const rclcpp::Time now_nanoseconds) const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
for (const auto & collector : subscriber_statistics_collectors_) {
|
||||
collector->OnMessageReceived(message_info, now_nanoseconds.nanoseconds());
|
||||
collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -22,7 +22,6 @@
|
||||
|
||||
#include "rcpputils/shared_library.hpp"
|
||||
#include "rosidl_runtime_cpp/message_type_support_decl.hpp"
|
||||
#include "rosidl_runtime_cpp/service_type_support_decl.hpp"
|
||||
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -41,15 +40,11 @@ get_typesupport_library(const std::string & type, const std::string & typesuppor
|
||||
/// Extract the type support handle from the library.
|
||||
/**
|
||||
* The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
|
||||
*
|
||||
* \deprecated Use get_message_typesupport_handle() instead
|
||||
*
|
||||
* \param[in] type The topic type, e.g. "std_msgs/msg/String"
|
||||
* \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
|
||||
* \param[in] library The shared type support library
|
||||
* \return A type support handle
|
||||
*/
|
||||
[[deprecated("Use `get_message_typesupport_handle` instead")]]
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_message_type_support_t *
|
||||
get_typesupport_handle(
|
||||
@@ -57,40 +52,6 @@ get_typesupport_handle(
|
||||
const std::string & typesupport_identifier,
|
||||
rcpputils::SharedLibrary & library);
|
||||
|
||||
/// Extract the message type support handle from the library.
|
||||
/**
|
||||
* The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
|
||||
*
|
||||
* \param[in] type The topic type, e.g. "std_msgs/msg/String"
|
||||
* \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
|
||||
* \param[in] library The shared type support library
|
||||
* \throws std::runtime_error if the symbol of type not found in the library.
|
||||
* \return A message type support handle
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_message_type_support_t *
|
||||
get_message_typesupport_handle(
|
||||
const std::string & type,
|
||||
const std::string & typesupport_identifier,
|
||||
rcpputils::SharedLibrary & library);
|
||||
|
||||
/// Extract the service type support handle from the library.
|
||||
/**
|
||||
* The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result.
|
||||
*
|
||||
* \param[in] type The service type, e.g. "std_srvs/srv/Empty"
|
||||
* \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp"
|
||||
* \param[in] library The shared type support library
|
||||
* \throws std::runtime_error if the symbol of type not found in the library.
|
||||
* \return A service type support handle
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_service_type_support_t *
|
||||
get_service_typesupport_handle(
|
||||
const std::string & type,
|
||||
const std::string & typesupport_identifier,
|
||||
rcpputils::SharedLibrary & library);
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__TYPESUPPORT_HELPERS_HPP_
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="2">
|
||||
<name>rclcpp</name>
|
||||
<version>28.0.0</version>
|
||||
<version>19.3.0</version>
|
||||
<description>The ROS client library in C++.</description>
|
||||
|
||||
<maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
|
||||
@@ -22,7 +22,6 @@
|
||||
<build_depend>builtin_interfaces</build_depend>
|
||||
<build_depend>rcl_interfaces</build_depend>
|
||||
<build_depend>rosgraph_msgs</build_depend>
|
||||
<build_depend>rosidl_runtime_c</build_depend>
|
||||
<build_depend>rosidl_runtime_cpp</build_depend>
|
||||
<build_depend>rosidl_typesupport_c</build_depend>
|
||||
<build_depend>rosidl_typesupport_cpp</build_depend>
|
||||
@@ -30,14 +29,12 @@
|
||||
<build_export_depend>builtin_interfaces</build_export_depend>
|
||||
<build_export_depend>rcl_interfaces</build_export_depend>
|
||||
<build_export_depend>rosgraph_msgs</build_export_depend>
|
||||
<build_export_depend>rosidl_runtime_c</build_export_depend>
|
||||
<build_export_depend>rosidl_runtime_cpp</build_export_depend>
|
||||
<build_export_depend>rosidl_typesupport_c</build_export_depend>
|
||||
<build_export_depend>rosidl_typesupport_cpp</build_export_depend>
|
||||
|
||||
<depend>libstatistics_collector</depend>
|
||||
<depend>rcl</depend>
|
||||
<depend>rcl_logging_interface</depend>
|
||||
<depend>rcl_yaml_param_parser</depend>
|
||||
<depend>rcpputils</depend>
|
||||
<depend>rcutils</depend>
|
||||
|
||||
@@ -31,12 +31,10 @@ using rclcpp::CallbackGroupType;
|
||||
|
||||
CallbackGroup::CallbackGroup(
|
||||
CallbackGroupType group_type,
|
||||
rclcpp::Context::WeakPtr context,
|
||||
bool automatically_add_to_executor_with_node)
|
||||
: type_(group_type), associated_with_executor_(false),
|
||||
can_be_taken_from_(true),
|
||||
automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node),
|
||||
context_(context)
|
||||
automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
|
||||
{}
|
||||
|
||||
CallbackGroup::~CallbackGroup()
|
||||
@@ -56,16 +54,6 @@ CallbackGroup::type() const
|
||||
return type_;
|
||||
}
|
||||
|
||||
size_t
|
||||
CallbackGroup::size() const
|
||||
{
|
||||
return
|
||||
subscription_ptrs_.size() +
|
||||
service_ptrs_.size() +
|
||||
client_ptrs_.size() +
|
||||
timer_ptrs_.size() +
|
||||
waitable_ptrs_.size();
|
||||
}
|
||||
void CallbackGroup::collect_all_ptrs(
|
||||
std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
|
||||
std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
|
||||
@@ -124,17 +112,21 @@ CallbackGroup::automatically_add_to_executor_with_node() const
|
||||
}
|
||||
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
CallbackGroup::get_notify_guard_condition()
|
||||
CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
|
||||
rclcpp::Context::SharedPtr context_ptr = context_.lock();
|
||||
if (context_ptr && context_ptr->is_valid()) {
|
||||
if (!notify_guard_condition_) {
|
||||
notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
|
||||
if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
|
||||
if (associated_with_executor_) {
|
||||
trigger_notify_guard_condition();
|
||||
}
|
||||
return notify_guard_condition_;
|
||||
notify_guard_condition_ = nullptr;
|
||||
}
|
||||
return nullptr;
|
||||
|
||||
if (!notify_guard_condition_) {
|
||||
notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
|
||||
}
|
||||
|
||||
return notify_guard_condition_;
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@@ -125,6 +125,7 @@ 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();
|
||||
@@ -137,7 +138,6 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
|
||||
// check was non-blocking, return immediately
|
||||
return false;
|
||||
}
|
||||
// make an event to reuse, rather than create a new one each time
|
||||
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()
|
||||
|
||||
@@ -462,7 +462,7 @@ template<Context::ShutdownType shutdown_type>
|
||||
std::vector<rclcpp::Context::ShutdownCallback>
|
||||
Context::get_shutdown_callback() const
|
||||
{
|
||||
const auto get_callback_vector = [](auto & mutex, auto & callback_set) {
|
||||
const auto get_callback_vector = [this](auto & mutex, auto & callback_set) {
|
||||
const std::lock_guard<std::mutex> lock(mutex);
|
||||
std::vector<rclcpp::Context::ShutdownCallback> callbacks;
|
||||
for (auto & callback : callback_set) {
|
||||
@@ -496,7 +496,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
|
||||
std::unique_lock<std::mutex> lock(interrupt_mutex_);
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
// this will release the lock while waiting
|
||||
interrupt_condition_variable_.wait_for(lock, time_left);
|
||||
interrupt_condition_variable_.wait_for(lock, nanoseconds);
|
||||
time_left -= std::chrono::steady_clock::now() - start;
|
||||
}
|
||||
} while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());
|
||||
|
||||
@@ -1,44 +0,0 @@
|
||||
// Copyright 2023 Sony Group Corporation.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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/create_generic_client.hpp"
|
||||
#include "rclcpp/generic_client.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
rclcpp::GenericClient::SharedPtr
|
||||
create_generic_client(
|
||||
std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
|
||||
std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
|
||||
std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
|
||||
const std::string & service_name,
|
||||
const std::string & service_type,
|
||||
const rclcpp::QoS & qos,
|
||||
rclcpp::CallbackGroup::SharedPtr group)
|
||||
{
|
||||
rcl_client_options_t options = rcl_client_get_default_options();
|
||||
options.qos = qos.get_rmw_qos_profile();
|
||||
|
||||
auto cli = rclcpp::GenericClient::make_shared(
|
||||
node_base.get(),
|
||||
node_graph,
|
||||
service_name,
|
||||
service_type,
|
||||
options);
|
||||
|
||||
auto cli_base_ptr = std::dynamic_pointer_cast<rclcpp::ClientBase>(cli);
|
||||
node_services->add_client(cli_base_ptr, group);
|
||||
return cli;
|
||||
}
|
||||
} // namespace rclcpp
|
||||
@@ -1,37 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <rclcpp/detail/resolve_intra_process_buffer_type.hpp>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
namespace detail
|
||||
{
|
||||
rclcpp::IntraProcessBufferType
|
||||
resolve_intra_process_buffer_type(
|
||||
const rclcpp::IntraProcessBufferType buffer_type)
|
||||
{
|
||||
if (buffer_type == IntraProcessBufferType::CallbackDefault) {
|
||||
throw std::invalid_argument(
|
||||
"IntraProcessBufferType::CallbackDefault is not allowed "
|
||||
"when there is no callback function");
|
||||
}
|
||||
|
||||
return buffer_type;
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
} // namespace rclcpp
|
||||
@@ -28,8 +28,6 @@
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rclcpp/utilities.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
@@ -318,50 +316,4 @@ Duration::from_nanoseconds(rcl_duration_value_t nanoseconds)
|
||||
return ret;
|
||||
}
|
||||
|
||||
builtin_interfaces::msg::Time
|
||||
operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs)
|
||||
{
|
||||
if (lhs.sec < 0) {
|
||||
throw std::runtime_error("message time is negative");
|
||||
}
|
||||
|
||||
rcl_time_point_value_t rcl_time;
|
||||
rcl_time = RCL_S_TO_NS(static_cast<int64_t>(lhs.sec));
|
||||
rcl_time += lhs.nanosec;
|
||||
|
||||
if (rclcpp::add_will_overflow(rcl_time, rhs.nanoseconds())) {
|
||||
throw std::overflow_error("addition leads to int64_t overflow");
|
||||
}
|
||||
if (rclcpp::add_will_underflow(rcl_time, rhs.nanoseconds())) {
|
||||
throw std::underflow_error("addition leads to int64_t underflow");
|
||||
}
|
||||
|
||||
rcl_time += rhs.nanoseconds();
|
||||
|
||||
return convert_rcl_time_to_sec_nanos(rcl_time);
|
||||
}
|
||||
|
||||
builtin_interfaces::msg::Time
|
||||
operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs)
|
||||
{
|
||||
if (lhs.sec < 0) {
|
||||
throw std::runtime_error("message time is negative");
|
||||
}
|
||||
|
||||
rcl_time_point_value_t rcl_time;
|
||||
rcl_time = RCL_S_TO_NS(static_cast<int64_t>(lhs.sec));
|
||||
rcl_time += lhs.nanosec;
|
||||
|
||||
if (rclcpp::sub_will_overflow(rcl_time, rhs.nanoseconds())) {
|
||||
throw std::overflow_error("addition leads to int64_t overflow");
|
||||
}
|
||||
if (rclcpp::sub_will_underflow(rcl_time, rhs.nanoseconds())) {
|
||||
throw std::underflow_error("addition leads to int64_t underflow");
|
||||
}
|
||||
|
||||
rcl_time -= rhs.nanoseconds();
|
||||
|
||||
return convert_rcl_time_to_sec_nanos(rcl_time);
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
113
rclcpp/src/rclcpp/dynamic_subscription.cpp
Normal file
113
rclcpp/src/rclcpp/dynamic_subscription.cpp
Normal file
@@ -0,0 +1,113 @@
|
||||
// Copyright 2022 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT 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/dynamic_subscription.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcl/subscription.h"
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
std::shared_ptr<void> DynamicSubscription::create_message()
|
||||
{
|
||||
return create_serialized_message();
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp::SerializedMessage> DynamicSubscription::create_serialized_message()
|
||||
{
|
||||
return std::make_shared<rclcpp::SerializedMessage>(0);
|
||||
}
|
||||
|
||||
void DynamicSubscription::handle_message(std::shared_ptr<void> &, const rclcpp::MessageInfo &)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"handle_message is not implemented for DynamicSubscription");
|
||||
}
|
||||
|
||||
void DynamicSubscription::handle_serialized_message(
|
||||
const std::shared_ptr<rclcpp::SerializedMessage> &, const rclcpp::MessageInfo &)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"handle_serialized_message is not implemented for DynamicSubscription");
|
||||
}
|
||||
|
||||
void DynamicSubscription::handle_loaned_message(void *, const rclcpp::MessageInfo &)
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"handle_loaned_message is not implemented for DynamicSubscription");
|
||||
}
|
||||
|
||||
void DynamicSubscription::return_message(std::shared_ptr<void> & message)
|
||||
{
|
||||
auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
|
||||
return_serialized_message(typed_message);
|
||||
}
|
||||
|
||||
void DynamicSubscription::return_serialized_message(
|
||||
std::shared_ptr<rclcpp::SerializedMessage> & message)
|
||||
{
|
||||
message.reset();
|
||||
}
|
||||
|
||||
|
||||
// DYNAMIC TYPE ====================================================================================
|
||||
// TODO(methylDragon): Re-order later
|
||||
|
||||
// Does not clone
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
|
||||
DynamicSubscription::get_shared_dynamic_message_type()
|
||||
{
|
||||
return dynamic_message_type_;
|
||||
}
|
||||
|
||||
// Does not clone
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
DynamicSubscription::get_shared_dynamic_message()
|
||||
{
|
||||
return dynamic_message_;
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
|
||||
DynamicSubscription::get_shared_dynamic_serialization_support()
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
DynamicSubscription::create_dynamic_message()
|
||||
{
|
||||
return dynamic_message_->init_from_type_shared(*dynamic_message_type_);
|
||||
}
|
||||
|
||||
void
|
||||
DynamicSubscription::return_dynamic_message(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
|
||||
{
|
||||
message.reset();
|
||||
}
|
||||
|
||||
void DynamicSubscription::handle_dynamic_message(
|
||||
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
|
||||
const rclcpp::MessageInfo & message_info)
|
||||
{
|
||||
(void) message_info;
|
||||
callback_(message, ts_->get_shared_rosidl_runtime_c_type_description());
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
||||
@@ -14,13 +14,11 @@
|
||||
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcl/allocator.h"
|
||||
#include "rcl/types.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
@@ -36,5 +34,805 @@ using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_IMPL_HPP_
|
||||
// Template specialization implementations
|
||||
#include "rclcpp/dynamic_typesupport/detail/dynamic_message_impl.hpp"
|
||||
#endif
|
||||
|
||||
|
||||
// CONSTRUCTION ==================================================================================
|
||||
DynamicMessage::DynamicMessage(const DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder)
|
||||
: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()),
|
||||
rosidl_dynamic_data_(nullptr),
|
||||
is_loaned_(false),
|
||||
parent_data_(nullptr)
|
||||
{
|
||||
if (!serialization_support_) {
|
||||
throw std::runtime_error("dynamic type could not bind serialization support!");
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder =
|
||||
dynamic_type_builder->get_rosidl_dynamic_type_builder();
|
||||
if (!rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error("dynamic type builder cannot be nullptr!");
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type_builder(
|
||||
rosidl_dynamic_type_builder, &rosidl_dynamic_data);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
|
||||
throw std::runtime_error("could not create new dynamic data object from dynamic type builder");
|
||||
}
|
||||
|
||||
rosidl_dynamic_data_.reset(
|
||||
rosidl_dynamic_data,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::DynamicMessage(const DynamicMessageType::SharedPtr dynamic_type)
|
||||
: serialization_support_(dynamic_type->get_shared_dynamic_serialization_support()),
|
||||
rosidl_dynamic_data_(nullptr),
|
||||
is_loaned_(false),
|
||||
parent_data_(nullptr)
|
||||
{
|
||||
if (!serialization_support_) {
|
||||
throw std::runtime_error("dynamic type could not bind serialization support!");
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type =
|
||||
dynamic_type->get_rosidl_dynamic_type();
|
||||
if (!rosidl_dynamic_type) {
|
||||
throw std::runtime_error("dynamic type cannot be nullptr!");
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type(
|
||||
rosidl_dynamic_type, &rosidl_dynamic_data);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not create new dynamic data object from dynamic type") +
|
||||
rcl_get_error_string().str);
|
||||
}
|
||||
|
||||
rosidl_dynamic_data_.reset(
|
||||
rosidl_dynamic_data,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::DynamicMessage(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)
|
||||
: serialization_support_(serialization_support),
|
||||
rosidl_dynamic_data_(nullptr),
|
||||
is_loaned_(false),
|
||||
parent_data_(nullptr)
|
||||
{
|
||||
if (!rosidl_dynamic_data) {
|
||||
throw std::runtime_error("rosidl dynamic data cannot be nullptr!");
|
||||
}
|
||||
if (serialization_support) {
|
||||
if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_data)) {
|
||||
throw std::runtime_error(
|
||||
"serialization support library identifier does not match dynamic data's!");
|
||||
}
|
||||
}
|
||||
|
||||
rosidl_dynamic_data_.reset(
|
||||
rosidl_dynamic_data,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_data_destroy(rosidl_dynamic_data);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::DynamicMessage(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t> rosidl_dynamic_data)
|
||||
: serialization_support_(serialization_support),
|
||||
rosidl_dynamic_data_(rosidl_dynamic_data),
|
||||
is_loaned_(false),
|
||||
parent_data_(nullptr)
|
||||
{
|
||||
if (!rosidl_dynamic_data) {
|
||||
throw std::runtime_error("rosidl dynamic data cannot be nullptr!");
|
||||
}
|
||||
if (serialization_support) {
|
||||
if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_data)) {
|
||||
throw std::runtime_error(
|
||||
"serialization support library identifier does not match dynamic data's!");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::DynamicMessage(
|
||||
DynamicMessage::SharedPtr parent_data,
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_loaned_data)
|
||||
: serialization_support_(parent_data->get_shared_dynamic_serialization_support()),
|
||||
rosidl_dynamic_data_(nullptr),
|
||||
is_loaned_(true),
|
||||
parent_data_(nullptr)
|
||||
{
|
||||
if (!parent_data) {
|
||||
throw std::runtime_error("parent dynamic data cannot be nullptr!");
|
||||
}
|
||||
if (!rosidl_loaned_data) {
|
||||
throw std::runtime_error("loaned rosidl dynamic data cannot be nullptr!");
|
||||
}
|
||||
|
||||
if (serialization_support_) {
|
||||
if (!match_serialization_support_(*serialization_support_, *rosidl_loaned_data)) {
|
||||
throw std::runtime_error(
|
||||
"serialization support library identifier does not match loaned dynamic data's!");
|
||||
}
|
||||
}
|
||||
|
||||
rosidl_dynamic_data_.reset(
|
||||
rosidl_loaned_data,
|
||||
// Custom no-op deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data)->void {
|
||||
// Data fini and destruction is deferred to return_loaned_value()
|
||||
(void) rosidl_dynamic_data;
|
||||
});
|
||||
parent_data_ = parent_data;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::DynamicMessage(const DynamicMessage & other)
|
||||
: enable_shared_from_this(),
|
||||
serialization_support_(nullptr),
|
||||
rosidl_dynamic_data_(nullptr),
|
||||
is_loaned_(false),
|
||||
parent_data_(nullptr)
|
||||
{
|
||||
DynamicMessage out = other.clone();
|
||||
// We don't copy is_loaned_ or parent_data_ because it's a fresh copy now
|
||||
std::swap(serialization_support_, out.serialization_support_);
|
||||
std::swap(rosidl_dynamic_data_, out.rosidl_dynamic_data_);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::DynamicMessage(DynamicMessage && other) noexcept
|
||||
: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
|
||||
rosidl_dynamic_data_(std::exchange(other.rosidl_dynamic_data_, nullptr)),
|
||||
is_loaned_(other.is_loaned_),
|
||||
parent_data_(std::exchange(other.parent_data_, nullptr))
|
||||
{}
|
||||
|
||||
|
||||
DynamicMessage &
|
||||
DynamicMessage::operator=(const DynamicMessage & other)
|
||||
{
|
||||
return *this = DynamicMessage(other);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage &
|
||||
DynamicMessage::operator=(DynamicMessage && other) noexcept
|
||||
{
|
||||
std::swap(serialization_support_, other.serialization_support_);
|
||||
std::swap(rosidl_dynamic_data_, other.rosidl_dynamic_data_);
|
||||
is_loaned_ = other.is_loaned_;
|
||||
std::swap(parent_data_, other.parent_data_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::~DynamicMessage()
|
||||
{} // STUBBED
|
||||
{
|
||||
if (is_loaned_) {
|
||||
if (!parent_data_) {
|
||||
RCUTILS_LOG_ERROR("dynamic data is loaned, but parent is missing!!");
|
||||
} else {
|
||||
rosidl_dynamic_typesupport_dynamic_data_return_loaned_value(
|
||||
parent_data_->get_rosidl_dynamic_data(), get_rosidl_dynamic_data());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
DynamicMessage::match_serialization_support_(
|
||||
const DynamicSerializationSupport & serialization_support,
|
||||
const rosidl_dynamic_typesupport_dynamic_data_t & rosidl_dynamic_type_data)
|
||||
{
|
||||
bool out = true;
|
||||
|
||||
if (serialization_support.get_library_identifier() != std::string(
|
||||
rosidl_dynamic_type_data.serialization_support->library_identifier))
|
||||
{
|
||||
RCUTILS_LOG_ERROR("serialization support library identifier does not match dynamic data's");
|
||||
out = false;
|
||||
}
|
||||
|
||||
// TODO(methylDragon): Can I do this?? Is it portable?
|
||||
if (serialization_support.get_rosidl_serialization_support() !=
|
||||
rosidl_dynamic_type_data.serialization_support)
|
||||
{
|
||||
RCUTILS_LOG_ERROR("serialization support pointer does not match dynamic data's");
|
||||
out = false;
|
||||
}
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// GETTERS =======================================================================================
|
||||
const std::string
|
||||
DynamicMessage::get_library_identifier() const
|
||||
{
|
||||
return std::string(rosidl_dynamic_data_->serialization_support->library_identifier);
|
||||
}
|
||||
|
||||
|
||||
const std::string
|
||||
DynamicMessage::get_name() const
|
||||
{
|
||||
size_t buf_length;
|
||||
const char * buf;
|
||||
if (
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_name(
|
||||
get_rosidl_dynamic_data(), &buf,
|
||||
&buf_length) !=
|
||||
RCUTILS_RET_OK)
|
||||
{
|
||||
throw std::runtime_error(
|
||||
std::string("could not get name for dynamic data") + rcl_get_error_string().str);
|
||||
}
|
||||
return std::string(buf, buf_length);
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_data_t *
|
||||
DynamicMessage::get_rosidl_dynamic_data()
|
||||
{
|
||||
return rosidl_dynamic_data_.get();
|
||||
}
|
||||
|
||||
|
||||
const rosidl_dynamic_typesupport_dynamic_data_t *
|
||||
DynamicMessage::get_rosidl_dynamic_data() const
|
||||
{
|
||||
return rosidl_dynamic_data_.get();
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>
|
||||
DynamicMessage::get_shared_rosidl_dynamic_data()
|
||||
{
|
||||
return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>(
|
||||
shared_from_this(), rosidl_dynamic_data_.get());
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_data_t>
|
||||
DynamicMessage::get_shared_rosidl_dynamic_data() const
|
||||
{
|
||||
return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_data_t>(
|
||||
shared_from_this(), rosidl_dynamic_data_.get());
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
DynamicMessage::get_shared_dynamic_serialization_support()
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
DynamicMessage::get_shared_dynamic_serialization_support() const
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
size_t
|
||||
DynamicMessage::get_item_count() const
|
||||
{
|
||||
size_t item_count;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_item_count(
|
||||
get_rosidl_dynamic_data(), &item_count);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error("could not get item count of dynamic data");
|
||||
}
|
||||
return item_count;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::get_member_id(size_t index) const
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t member_id;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_at_index(
|
||||
get_rosidl_dynamic_data(), index, &member_id);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error("could not member id of dynamic data element by index");
|
||||
}
|
||||
return member_id;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::get_member_id(const std::string & name) const
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t member_id;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_member_id_by_name(
|
||||
get_rosidl_dynamic_data(), name.c_str(), name.size(), &member_id);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error("could not member id of dynamic data element by name");
|
||||
}
|
||||
return member_id;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::get_array_index(size_t index) const
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t array_index;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_get_array_index(
|
||||
get_rosidl_dynamic_data(), index, &array_index);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error("could not array index of dynamic data element by index");
|
||||
}
|
||||
return array_index;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::get_array_index(const std::string & name) const
|
||||
{
|
||||
return get_array_index(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
// METHODS =======================================================================================
|
||||
DynamicMessage
|
||||
DynamicMessage::clone() const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone(
|
||||
get_rosidl_dynamic_data(), &rosidl_dynamic_data);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not clone dynamic data: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return DynamicMessage(serialization_support_, rosidl_dynamic_data);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessage::clone_shared() const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_clone(
|
||||
get_rosidl_dynamic_data(), &rosidl_dynamic_data);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not clone dynamic data: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return DynamicMessage::make_shared(serialization_support_, rosidl_dynamic_data);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage
|
||||
DynamicMessage::init_from_type(DynamicMessageType & type) const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type(
|
||||
type.get_rosidl_dynamic_type(), &rosidl_dynamic_data);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
|
||||
throw std::runtime_error("could not create new dynamic data object from dynamic type");
|
||||
}
|
||||
return DynamicMessage(serialization_support_, rosidl_dynamic_data);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessage::init_from_type_shared(DynamicMessageType & type) const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_create_from_dynamic_type(
|
||||
type.get_rosidl_dynamic_type(), &rosidl_dynamic_data);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
|
||||
throw std::runtime_error("could not create new dynamic data object from dynamic type");
|
||||
}
|
||||
return DynamicMessage::make_shared(serialization_support_, rosidl_dynamic_data);
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
DynamicMessage::equals(const DynamicMessage & other) const
|
||||
{
|
||||
if (get_library_identifier() != other.get_library_identifier()) {
|
||||
throw std::runtime_error("library identifiers don't match");
|
||||
}
|
||||
bool equals;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_equals(
|
||||
get_rosidl_dynamic_data(), other.get_rosidl_dynamic_data(), &equals);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error("could not equate dynamic messages");
|
||||
}
|
||||
return equals;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessage::loan_value(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * rosidl_dynamic_data = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_data_loan_value(
|
||||
get_rosidl_dynamic_data(), id, &rosidl_dynamic_data);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_data) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not loan dynamic data: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return DynamicMessage::make_shared(shared_from_this(), rosidl_dynamic_data);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessage::loan_value(const std::string & name)
|
||||
{
|
||||
return loan_value(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::clear_all_values()
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_clear_all_values(get_rosidl_dynamic_data());
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::clear_nonkey_values()
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_clear_nonkey_values(get_rosidl_dynamic_data());
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::clear_value(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_clear_value(get_rosidl_dynamic_data(), id);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::clear_value(const std::string & name)
|
||||
{
|
||||
clear_value(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::clear_sequence()
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_clear_sequence_data(get_rosidl_dynamic_data());
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_sequence_data()
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_sequence_data(get_rosidl_dynamic_data(), &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::remove_sequence_data(rosidl_dynamic_typesupport_member_id_t index)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_remove_sequence_data(
|
||||
get_rosidl_dynamic_data(), index);
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
DynamicMessage::serialize(rcl_serialized_message_t * buffer)
|
||||
{
|
||||
bool success;
|
||||
rcutils_ret_t ret =
|
||||
rosidl_dynamic_typesupport_dynamic_data_serialize(get_rosidl_dynamic_data(), buffer, &success);
|
||||
if (ret != RCUTILS_RET_OK || !success) {
|
||||
throw std::runtime_error(
|
||||
std::string("could serialize loan dynamic data: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
DynamicMessage::deserialize(rcl_serialized_message_t * buffer)
|
||||
{
|
||||
bool success;
|
||||
rcutils_ret_t ret =
|
||||
rosidl_dynamic_typesupport_dynamic_data_deserialize(
|
||||
get_rosidl_dynamic_data(), buffer,
|
||||
&success);
|
||||
if (ret != RCUTILS_RET_OK || !success) {
|
||||
throw std::runtime_error(
|
||||
std::string("could deserialize loan dynamic data: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
// MEMBER ACCESS ===================================================================================
|
||||
// Defined in "detail/dynamic_message_impl.hpp"
|
||||
|
||||
|
||||
// FIXED STRING MEMBER ACCESS ======================================================================
|
||||
const std::string
|
||||
DynamicMessage::get_fixed_string_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, size_t string_length)
|
||||
{
|
||||
size_t buf_length;
|
||||
char * buf = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_fixed_string_value(
|
||||
get_rosidl_dynamic_data(), id, &buf, &buf_length, string_length);
|
||||
auto out = std::string(buf, buf_length);
|
||||
delete buf;
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
const std::string
|
||||
DynamicMessage::get_fixed_string_value(const std::string & name, size_t string_length)
|
||||
{
|
||||
return get_fixed_string_value(get_member_id(name), string_length);
|
||||
}
|
||||
|
||||
|
||||
const std::u16string
|
||||
DynamicMessage::get_fixed_wstring_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, size_t wstring_length)
|
||||
{
|
||||
size_t buf_length;
|
||||
char16_t * buf = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_fixed_wstring_value(
|
||||
get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_length);
|
||||
auto out = std::u16string(buf, buf_length);
|
||||
delete buf;
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
const std::u16string
|
||||
DynamicMessage::get_fixed_wstring_value(const std::string & name, size_t wstring_length)
|
||||
{
|
||||
return get_fixed_wstring_value(get_member_id(name), wstring_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_fixed_string_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_length)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_fixed_string_value(
|
||||
get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_fixed_string_value(
|
||||
const std::string & name, const std::string value, size_t string_length)
|
||||
{
|
||||
set_fixed_string_value(get_member_id(name), value, string_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_fixed_wstring_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_length)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_fixed_wstring_value(
|
||||
get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_fixed_wstring_value(
|
||||
const std::string & name, const std::u16string value, size_t wstring_length)
|
||||
{
|
||||
set_fixed_wstring_value(get_member_id(name), value, wstring_length);
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_fixed_string_value(const std::string value, size_t string_length)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_fixed_string_value(
|
||||
get_rosidl_dynamic_data(), value.c_str(), value.size(), string_length, &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_fixed_wstring_value(const std::u16string value, size_t wstring_length)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_fixed_wstring_value(
|
||||
get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_length, &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// BOUNDED STRING MEMBER ACCESS ====================================================================
|
||||
const std::string
|
||||
DynamicMessage::get_bounded_string_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, size_t string_bound)
|
||||
{
|
||||
size_t buf_length;
|
||||
char * buf = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_bounded_string_value(
|
||||
get_rosidl_dynamic_data(), id, &buf, &buf_length, string_bound);
|
||||
auto out = std::string(buf, buf_length);
|
||||
delete buf;
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
const std::string
|
||||
DynamicMessage::get_bounded_string_value(const std::string & name, size_t string_bound)
|
||||
{
|
||||
return get_bounded_string_value(get_member_id(name), string_bound);
|
||||
}
|
||||
|
||||
|
||||
const std::u16string
|
||||
DynamicMessage::get_bounded_wstring_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, size_t wstring_bound)
|
||||
{
|
||||
size_t buf_length;
|
||||
char16_t * buf = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_bounded_wstring_value(
|
||||
get_rosidl_dynamic_data(), id, &buf, &buf_length, wstring_bound);
|
||||
auto out = std::u16string(buf, buf_length);
|
||||
delete buf;
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
const std::u16string
|
||||
DynamicMessage::get_bounded_wstring_value(const std::string & name, size_t wstring_bound)
|
||||
{
|
||||
return get_bounded_wstring_value(get_member_id(name), wstring_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_bounded_string_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string value, size_t string_bound)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_bounded_string_value(
|
||||
get_rosidl_dynamic_data(), id, value.c_str(), value.size(), string_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_bounded_string_value(
|
||||
const std::string & name, const std::string value, size_t string_bound)
|
||||
{
|
||||
set_bounded_string_value(get_member_id(name), value, string_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_bounded_wstring_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::u16string value, size_t wstring_bound)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_bounded_wstring_value(
|
||||
get_rosidl_dynamic_data(), id, value.c_str(), value.size(), wstring_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_bounded_wstring_value(
|
||||
const std::string & name, const std::u16string value, size_t wstring_bound)
|
||||
{
|
||||
set_bounded_wstring_value(get_member_id(name), value, wstring_bound);
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_bounded_string_value(const std::string value, size_t string_bound)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_bounded_string_value(
|
||||
get_rosidl_dynamic_data(), value.c_str(), value.size(), string_bound, &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_bounded_wstring_value(const std::u16string value, size_t wstring_bound)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_bounded_wstring_value(
|
||||
get_rosidl_dynamic_data(), value.c_str(), value.size(), wstring_bound, &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// NESTED MEMBER ACCESS ============================================================================
|
||||
DynamicMessage
|
||||
DynamicMessage::get_complex_value(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_complex_value(
|
||||
get_rosidl_dynamic_data(), id, &out_ptr);
|
||||
return DynamicMessage(get_shared_dynamic_serialization_support(), out_ptr);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage
|
||||
DynamicMessage::get_complex_value(const std::string & name)
|
||||
{
|
||||
return get_complex_value(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessage::get_complex_value_shared(rosidl_dynamic_typesupport_member_id_t id)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_t * out_ptr = nullptr;
|
||||
rosidl_dynamic_typesupport_dynamic_data_get_complex_value(
|
||||
get_rosidl_dynamic_data(), id, &out_ptr);
|
||||
return DynamicMessage::make_shared(get_shared_dynamic_serialization_support(), out_ptr);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessage::get_complex_value_shared(const std::string & name)
|
||||
{
|
||||
return get_complex_value_shared(get_member_id(name));
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_complex_value(
|
||||
rosidl_dynamic_typesupport_member_id_t id, DynamicMessage & value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_data_set_complex_value(
|
||||
get_rosidl_dynamic_data(), id, value.get_rosidl_dynamic_data());
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessage::set_complex_value(const std::string & name, DynamicMessage & value)
|
||||
{
|
||||
set_complex_value(get_member_id(name), value);
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_complex_value_copy(const DynamicMessage & value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_complex_value_copy(
|
||||
get_rosidl_dynamic_data(), value.get_rosidl_dynamic_data(), &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_member_id_t
|
||||
DynamicMessage::insert_complex_value(DynamicMessage & value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_member_id_t out;
|
||||
rosidl_dynamic_typesupport_dynamic_data_insert_complex_value(
|
||||
get_rosidl_dynamic_data(), value.get_rosidl_dynamic_data(), &out);
|
||||
return out;
|
||||
}
|
||||
|
||||
@@ -14,7 +14,6 @@
|
||||
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
@@ -34,5 +33,306 @@ using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
DynamicMessageType::~DynamicMessageType()
|
||||
{} // STUBBED
|
||||
|
||||
// CONSTRUCTION ====================================================================================
|
||||
DynamicMessageType::DynamicMessageType(DynamicMessageTypeBuilder::SharedPtr dynamic_type_builder)
|
||||
: serialization_support_(dynamic_type_builder->get_shared_dynamic_serialization_support()),
|
||||
rosidl_dynamic_type_(nullptr)
|
||||
{
|
||||
if (!serialization_support_) {
|
||||
throw std::runtime_error("dynamic type could not bind serialization support!");
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder =
|
||||
dynamic_type_builder->get_rosidl_dynamic_type_builder();
|
||||
if (!rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error("dynamic type builder cannot be nullptr!");
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_create_from_dynamic_type_builder(
|
||||
rosidl_dynamic_type_builder, &rosidl_dynamic_type);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
|
||||
throw std::runtime_error("could not create new dynamic type object");
|
||||
}
|
||||
|
||||
rosidl_dynamic_type_.reset(
|
||||
rosidl_dynamic_type,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::DynamicMessageType(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)
|
||||
: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr)
|
||||
{
|
||||
if (!rosidl_dynamic_type) {
|
||||
throw std::runtime_error("rosidl dynamic type cannot be nullptr!");
|
||||
}
|
||||
if (serialization_support) {
|
||||
if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type)) {
|
||||
throw std::runtime_error(
|
||||
"serialization support library identifier does not match dynamic type's!");
|
||||
}
|
||||
}
|
||||
|
||||
rosidl_dynamic_type_.reset(
|
||||
rosidl_dynamic_type,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::DynamicMessageType(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t> rosidl_dynamic_type)
|
||||
: serialization_support_(serialization_support), rosidl_dynamic_type_(rosidl_dynamic_type)
|
||||
{
|
||||
if (!rosidl_dynamic_type) {
|
||||
throw std::runtime_error("rosidl dynamic type cannot be nullptr!");
|
||||
}
|
||||
if (serialization_support) {
|
||||
if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type)) {
|
||||
throw std::runtime_error(
|
||||
"serialization support library identifier does not match dynamic type's!");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::DynamicMessageType(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description)
|
||||
: serialization_support_(serialization_support), rosidl_dynamic_type_(nullptr)
|
||||
{
|
||||
init_from_description(description, serialization_support);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::DynamicMessageType(const DynamicMessageType & other)
|
||||
: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_(nullptr)
|
||||
{
|
||||
DynamicMessageType out = other.clone();
|
||||
std::swap(serialization_support_, out.serialization_support_);
|
||||
std::swap(rosidl_dynamic_type_, out.rosidl_dynamic_type_);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::DynamicMessageType(DynamicMessageType && other) noexcept
|
||||
: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
|
||||
rosidl_dynamic_type_(std::exchange(other.rosidl_dynamic_type_, nullptr)) {}
|
||||
|
||||
|
||||
DynamicMessageType &
|
||||
DynamicMessageType::operator=(const DynamicMessageType & other)
|
||||
{
|
||||
return *this = DynamicMessageType(other);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType &
|
||||
DynamicMessageType::operator=(DynamicMessageType && other) noexcept
|
||||
{
|
||||
std::swap(serialization_support_, other.serialization_support_);
|
||||
std::swap(rosidl_dynamic_type_, other.rosidl_dynamic_type_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::~DynamicMessageType() {}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageType::init_from_description(
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description,
|
||||
DynamicSerializationSupport::SharedPtr serialization_support)
|
||||
{
|
||||
if (serialization_support) {
|
||||
// Swap serialization support if serialization support is given
|
||||
serialization_support_ = serialization_support;
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_create_from_description(
|
||||
serialization_support_->get_rosidl_serialization_support(), &description, &rosidl_dynamic_type);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
|
||||
throw std::runtime_error("could not create new dynamic type object");
|
||||
}
|
||||
|
||||
rosidl_dynamic_type_.reset(
|
||||
rosidl_dynamic_type,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_type_destroy(rosidl_dynamic_type);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
DynamicMessageType::match_serialization_support_(
|
||||
const DynamicSerializationSupport & serialization_support,
|
||||
const rosidl_dynamic_typesupport_dynamic_type_t & rosidl_dynamic_type)
|
||||
{
|
||||
bool out = true;
|
||||
|
||||
if (serialization_support.get_library_identifier() != std::string(
|
||||
rosidl_dynamic_type.serialization_support->library_identifier))
|
||||
{
|
||||
RCUTILS_LOG_ERROR(
|
||||
"serialization support library identifier does not match dynamic type's");
|
||||
out = false;
|
||||
}
|
||||
|
||||
// TODO(methylDragon): Can I do this?? Is it portable?
|
||||
if (serialization_support.get_rosidl_serialization_support() !=
|
||||
rosidl_dynamic_type.serialization_support)
|
||||
{
|
||||
RCUTILS_LOG_ERROR(
|
||||
"serialization support pointer does not match dynamic type's");
|
||||
out = false;
|
||||
}
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// GETTERS =========================================================================================
|
||||
const std::string
|
||||
DynamicMessageType::get_library_identifier() const
|
||||
{
|
||||
return std::string(rosidl_dynamic_type_->serialization_support->library_identifier);
|
||||
}
|
||||
|
||||
|
||||
const std::string
|
||||
DynamicMessageType::get_name() const
|
||||
{
|
||||
size_t buf_length;
|
||||
const char * buf;
|
||||
rosidl_dynamic_typesupport_dynamic_type_get_name(get_rosidl_dynamic_type(), &buf, &buf_length);
|
||||
return std::string(buf, buf_length);
|
||||
}
|
||||
|
||||
|
||||
size_t
|
||||
DynamicMessageType::get_member_count() const
|
||||
{
|
||||
size_t out;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_get_member_count(
|
||||
rosidl_dynamic_type_.get(), &out);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not get member count: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_t *
|
||||
DynamicMessageType::get_rosidl_dynamic_type()
|
||||
{
|
||||
return rosidl_dynamic_type_.get();
|
||||
}
|
||||
|
||||
|
||||
const rosidl_dynamic_typesupport_dynamic_type_t *
|
||||
DynamicMessageType::get_rosidl_dynamic_type() const
|
||||
{
|
||||
return rosidl_dynamic_type_.get();
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>
|
||||
DynamicMessageType::get_shared_rosidl_dynamic_type()
|
||||
{
|
||||
return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>(
|
||||
shared_from_this(), rosidl_dynamic_type_.get());
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_t>
|
||||
DynamicMessageType::get_shared_rosidl_dynamic_type() const
|
||||
{
|
||||
return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_t>(
|
||||
shared_from_this(), rosidl_dynamic_type_.get());
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
DynamicMessageType::get_shared_dynamic_serialization_support()
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
DynamicMessageType::get_shared_dynamic_serialization_support() const
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
// METHODS =========================================================================================
|
||||
DynamicMessageType
|
||||
DynamicMessageType::clone() const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone(
|
||||
get_rosidl_dynamic_type(), &rosidl_dynamic_type);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not clone dynamic type: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return DynamicMessageType(serialization_support_, rosidl_dynamic_type);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::SharedPtr
|
||||
DynamicMessageType::clone_shared() const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_t * rosidl_dynamic_type = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_clone(
|
||||
get_rosidl_dynamic_type(), &rosidl_dynamic_type);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not clone dynamic type: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return DynamicMessageType::make_shared(serialization_support_, rosidl_dynamic_type);
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
DynamicMessageType::equals(const DynamicMessageType & other) const
|
||||
{
|
||||
if (get_library_identifier() != other.get_library_identifier()) {
|
||||
throw std::runtime_error("library identifiers don't match");
|
||||
}
|
||||
bool out;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_equals(
|
||||
get_rosidl_dynamic_type(), other.get_rosidl_dynamic_type(), &out);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not equate dynamic message types: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage
|
||||
DynamicMessageType::build_dynamic_message()
|
||||
{
|
||||
return DynamicMessage(shared_from_this());
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessageType::build_dynamic_message_shared()
|
||||
{
|
||||
return DynamicMessage::make_shared(shared_from_this());
|
||||
}
|
||||
|
||||
@@ -14,7 +14,6 @@
|
||||
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
@@ -28,10 +27,649 @@
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicMessage;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder()
|
||||
{} // STUBBED
|
||||
|
||||
#ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DETAIL__DYNAMIC_MESSAGE_TYPE_BUILDER_IMPL_HPP_
|
||||
// Template specialization implementations
|
||||
#include "rclcpp/dynamic_typesupport/detail/dynamic_message_type_builder_impl.hpp"
|
||||
#endif
|
||||
|
||||
|
||||
// CONSTRUCTION ====================================================================================
|
||||
DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support, const std::string & name)
|
||||
: serialization_support_(serialization_support), rosidl_dynamic_type_builder_(nullptr)
|
||||
{
|
||||
init_from_serialization_support_(serialization_support, name);
|
||||
if (!rosidl_dynamic_type_builder_) {
|
||||
throw std::runtime_error("could not create new dynamic type builder object");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)
|
||||
: serialization_support_(serialization_support), rosidl_dynamic_type_builder_(nullptr)
|
||||
{
|
||||
if (!serialization_support) {
|
||||
throw std::runtime_error("serialization support cannot be nullptr!");
|
||||
}
|
||||
if (!rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error("rosidl dynamic type builder cannot be nullptr!");
|
||||
}
|
||||
if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type_builder)) {
|
||||
throw std::runtime_error(
|
||||
"serialization support library does not match dynamic type builder's!");
|
||||
}
|
||||
|
||||
rosidl_dynamic_type_builder_.reset(
|
||||
rosidl_dynamic_type_builder,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t> rosidl_dynamic_type_builder)
|
||||
: serialization_support_(serialization_support),
|
||||
rosidl_dynamic_type_builder_(rosidl_dynamic_type_builder)
|
||||
{
|
||||
if (!serialization_support) {
|
||||
throw std::runtime_error("serialization support cannot be nullptr!");
|
||||
}
|
||||
if (!rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error("rosidl dynamic type builder cannot be nullptr!");
|
||||
}
|
||||
if (!match_serialization_support_(*serialization_support, *rosidl_dynamic_type_builder.get())) {
|
||||
throw std::runtime_error(
|
||||
"serialization support library does not match dynamic type builder's!");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description)
|
||||
: serialization_support_(serialization_support),
|
||||
rosidl_dynamic_type_builder_(nullptr)
|
||||
{
|
||||
if (!serialization_support) {
|
||||
throw std::runtime_error("serialization support cannot be nullptr!");
|
||||
}
|
||||
init_from_description(description, serialization_support);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(const DynamicMessageTypeBuilder & other)
|
||||
: enable_shared_from_this(), serialization_support_(nullptr), rosidl_dynamic_type_builder_(nullptr)
|
||||
{
|
||||
DynamicMessageTypeBuilder out = other.clone();
|
||||
std::swap(serialization_support_, out.serialization_support_);
|
||||
std::swap(rosidl_dynamic_type_builder_, out.rosidl_dynamic_type_builder_);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder::DynamicMessageTypeBuilder(DynamicMessageTypeBuilder && other) noexcept
|
||||
: serialization_support_(std::exchange(other.serialization_support_, nullptr)),
|
||||
rosidl_dynamic_type_builder_(std::exchange(other.rosidl_dynamic_type_builder_, nullptr)) {}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder &
|
||||
DynamicMessageTypeBuilder::operator=(const DynamicMessageTypeBuilder & other)
|
||||
{
|
||||
return *this = DynamicMessageTypeBuilder(other);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder &
|
||||
DynamicMessageTypeBuilder::operator=(DynamicMessageTypeBuilder && other) noexcept
|
||||
{
|
||||
std::swap(serialization_support_, other.serialization_support_);
|
||||
std::swap(rosidl_dynamic_type_builder_, other.rosidl_dynamic_type_builder_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder() {}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::init_from_description(
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description,
|
||||
DynamicSerializationSupport::SharedPtr serialization_support)
|
||||
{
|
||||
if (serialization_support) {
|
||||
// Swap serialization support if serialization support is given
|
||||
serialization_support_ = serialization_support;
|
||||
}
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_create_from_description(
|
||||
serialization_support_->get_rosidl_serialization_support(), &description,
|
||||
&rosidl_dynamic_type_builder);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error("could not create new dynamic type builder object");
|
||||
}
|
||||
|
||||
rosidl_dynamic_type_builder_.reset(
|
||||
rosidl_dynamic_type_builder,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::init_from_serialization_support_(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const std::string & name)
|
||||
{
|
||||
if (!serialization_support) {
|
||||
throw std::runtime_error("serialization support cannot be nullptr!");
|
||||
}
|
||||
if (!serialization_support->get_rosidl_serialization_support()) {
|
||||
throw std::runtime_error("serialization support raw pointer cannot be nullptr!");
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_create(
|
||||
serialization_support->get_rosidl_serialization_support(),
|
||||
name.c_str(), name.size(),
|
||||
&rosidl_dynamic_type_builder);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not init dynamic type builder: ") + rcl_get_error_string().str);
|
||||
}
|
||||
if (!rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error("could not init dynamic type builder object");
|
||||
}
|
||||
|
||||
rosidl_dynamic_type_builder_.reset(
|
||||
rosidl_dynamic_type_builder,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder)->void {
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_destroy(rosidl_dynamic_type_builder);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
DynamicMessageTypeBuilder::match_serialization_support_(
|
||||
const DynamicSerializationSupport & serialization_support,
|
||||
const rosidl_dynamic_typesupport_dynamic_type_builder_t & rosidl_dynamic_type_builder)
|
||||
{
|
||||
bool out = true;
|
||||
|
||||
if (serialization_support.get_library_identifier() != std::string(
|
||||
rosidl_dynamic_type_builder.serialization_support->library_identifier))
|
||||
{
|
||||
RCUTILS_LOG_ERROR(
|
||||
"serialization support library identifier does not match dynamic type builder's");
|
||||
out = false;
|
||||
}
|
||||
|
||||
// TODO(methylDragon): Can I do this?? Is it portable?
|
||||
if (serialization_support.get_rosidl_serialization_support() !=
|
||||
rosidl_dynamic_type_builder.serialization_support)
|
||||
{
|
||||
RCUTILS_LOG_ERROR(
|
||||
"serialization support pointer does not match dynamic type builder's");
|
||||
out = false;
|
||||
}
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// GETTERS =======================================================================================
|
||||
const std::string
|
||||
DynamicMessageTypeBuilder::get_library_identifier() const
|
||||
{
|
||||
return std::string(rosidl_dynamic_type_builder_->serialization_support->library_identifier);
|
||||
}
|
||||
|
||||
|
||||
const std::string
|
||||
DynamicMessageTypeBuilder::get_name() const
|
||||
{
|
||||
size_t buf_length;
|
||||
const char * buf;
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_get_name(
|
||||
get_rosidl_dynamic_type_builder(), &buf, &buf_length);
|
||||
return std::string(buf, buf_length);
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t *
|
||||
DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder()
|
||||
{
|
||||
return rosidl_dynamic_type_builder_.get();
|
||||
}
|
||||
|
||||
|
||||
const rosidl_dynamic_typesupport_dynamic_type_builder_t *
|
||||
DynamicMessageTypeBuilder::get_rosidl_dynamic_type_builder() const
|
||||
{
|
||||
return rosidl_dynamic_type_builder_.get();
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>
|
||||
DynamicMessageTypeBuilder::get_shared_rosidl_dynamic_type_builder()
|
||||
{
|
||||
return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>(
|
||||
shared_from_this(), rosidl_dynamic_type_builder_.get());
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_dynamic_type_builder_t>
|
||||
DynamicMessageTypeBuilder::get_shared_rosidl_dynamic_type_builder() const
|
||||
{
|
||||
return std::shared_ptr<rosidl_dynamic_typesupport_dynamic_type_builder_t>(
|
||||
shared_from_this(), rosidl_dynamic_type_builder_.get());
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support()
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
DynamicMessageTypeBuilder::get_shared_dynamic_serialization_support() const
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
// METHODS =======================================================================================
|
||||
void
|
||||
DynamicMessageTypeBuilder::set_name(const std::string & name)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_set_name(
|
||||
get_rosidl_dynamic_type_builder(), name.c_str(), name.size());
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder
|
||||
DynamicMessageTypeBuilder::clone() const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone(
|
||||
get_rosidl_dynamic_type_builder(), &rosidl_dynamic_type_builder);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return DynamicMessageTypeBuilder(serialization_support_, rosidl_dynamic_type_builder);
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeBuilder::SharedPtr
|
||||
DynamicMessageTypeBuilder::clone_shared() const
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t * rosidl_dynamic_type_builder = nullptr;
|
||||
rcutils_ret_t ret = rosidl_dynamic_typesupport_dynamic_type_builder_clone(
|
||||
get_rosidl_dynamic_type_builder(), &rosidl_dynamic_type_builder);
|
||||
if (ret != RCUTILS_RET_OK || !rosidl_dynamic_type_builder) {
|
||||
throw std::runtime_error(
|
||||
std::string("could not clone dynamic type builder: ") + rcl_get_error_string().str);
|
||||
}
|
||||
return DynamicMessageTypeBuilder::make_shared(
|
||||
serialization_support_, rosidl_dynamic_type_builder);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::clear()
|
||||
{
|
||||
if (!serialization_support_) {
|
||||
throw std::runtime_error(
|
||||
"cannot call clear() on a dynamic type builder with uninitialized serialization support"
|
||||
);
|
||||
}
|
||||
|
||||
const std::string & name = get_name();
|
||||
init_from_serialization_support_(serialization_support_, name);
|
||||
if (!rosidl_dynamic_type_builder_) {
|
||||
throw std::runtime_error("could not create new dynamic type builder object");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage
|
||||
DynamicMessageTypeBuilder::build_dynamic_message()
|
||||
{
|
||||
return DynamicMessage(shared_from_this());
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessageTypeBuilder::build_dynamic_message_shared()
|
||||
{
|
||||
return DynamicMessage::make_shared(shared_from_this());
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType
|
||||
DynamicMessageTypeBuilder::build_dynamic_message_type()
|
||||
{
|
||||
return DynamicMessageType(shared_from_this());
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::SharedPtr
|
||||
DynamicMessageTypeBuilder::build_dynamic_message_type_shared()
|
||||
{
|
||||
return DynamicMessageType::make_shared(shared_from_this());
|
||||
}
|
||||
|
||||
|
||||
// ADD MEMBERS =====================================================================================
|
||||
// Defined in "detail/dynamic_message_type_builder_impl.hpp"
|
||||
|
||||
|
||||
// ADD FIXED STRING MEMBERS ========================================================================
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_string_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_wstring_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_string_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_length, size_t array_length, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_array_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_length, array_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_wstring_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_length, size_t array_length, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_array_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_length, array_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_string_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_length,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_unbounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_wstring_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_length,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_unbounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_string_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_length, size_t sequence_bound, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_string_bounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_length, sequence_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_fixed_wstring_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_length, size_t sequence_bound, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_fixed_wstring_bounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_length, sequence_bound);
|
||||
}
|
||||
|
||||
|
||||
// ADD BOUNDED STRING MEMBERS ======================================================================
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_string_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_wstring_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_string_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_bound, size_t array_length, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_array_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_bound, array_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_wstring_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_bound, size_t array_length, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_array_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_bound, array_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_string_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t string_bound,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_unbounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_wstring_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name, size_t wstring_bound,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_unbounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_string_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t string_bound, size_t sequence_bound, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_string_bounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
string_bound, sequence_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_bounded_wstring_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
size_t wstring_bound, size_t sequence_bound, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_bounded_wstring_bounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
wstring_bound, sequence_bound);
|
||||
}
|
||||
|
||||
|
||||
// ADD NESTED MEMBERS ==============================================================================
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type.get_rosidl_dynamic_type());
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_array_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, size_t array_length, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type.get_rosidl_dynamic_type(), array_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type.get_rosidl_dynamic_type());
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_bounded_sequence_member(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageType & nested_type, size_t sequence_bound, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type.get_rosidl_dynamic_type(), sequence_bound);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_member_builder(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type_builder.get_rosidl_dynamic_type_builder());
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_array_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, size_t array_length,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_array_member_builder(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type_builder.get_rosidl_dynamic_type_builder(), array_length);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_unbounded_sequence_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_unbounded_sequence_member_builder(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type_builder.get_rosidl_dynamic_type_builder());
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeBuilder::add_complex_bounded_sequence_member_builder(
|
||||
rosidl_dynamic_typesupport_member_id_t id, const std::string & name,
|
||||
DynamicMessageTypeBuilder & nested_type_builder, size_t sequence_bound,
|
||||
const std::string & default_value)
|
||||
{
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_add_complex_bounded_sequence_member_builder(
|
||||
get_rosidl_dynamic_type_builder(),
|
||||
id, name.c_str(), name.size(), default_value.c_str(), default_value.size(),
|
||||
nested_type_builder.get_rosidl_dynamic_type_builder(), sequence_bound);
|
||||
}
|
||||
|
||||
@@ -18,8 +18,6 @@
|
||||
#include <rosidl_runtime_c/type_description_utils.h>
|
||||
#include <rosidl_runtime_c/type_description/type_description__functions.h>
|
||||
#include <rosidl_runtime_c/type_description/type_description__struct.h>
|
||||
#include <rosidl_runtime_c/type_description/type_source__functions.h>
|
||||
#include <rosidl_runtime_c/type_description/type_source__struct.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
@@ -40,10 +38,428 @@
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicMessage;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
DynamicMessageTypeSupport::~DynamicMessageTypeSupport()
|
||||
{} // STUBBED
|
||||
|
||||
// CONSTRUCTION ====================================================================================
|
||||
DynamicMessageTypeSupport::DynamicMessageTypeSupport(
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description,
|
||||
const std::string & serialization_library_name)
|
||||
: serialization_support_(nullptr),
|
||||
dynamic_message_type_(nullptr),
|
||||
dynamic_message_(nullptr),
|
||||
description_(nullptr),
|
||||
rosidl_message_type_support_(nullptr)
|
||||
{
|
||||
rosidl_message_type_support_t * ts = nullptr;
|
||||
rcl_ret_t ret;
|
||||
|
||||
if (serialization_library_name.empty()) {
|
||||
ret = rcl_dynamic_message_type_support_handle_create(nullptr, &description, &ts);
|
||||
} else {
|
||||
ret = rcl_dynamic_message_type_support_handle_create(
|
||||
serialization_library_name.c_str(), &description, &ts);
|
||||
}
|
||||
if (ret != RCL_RET_OK) {
|
||||
throw std::runtime_error("error initializing rosidl message type support");
|
||||
}
|
||||
if (!ts) {
|
||||
throw std::runtime_error("could not init rosidl message type support");
|
||||
}
|
||||
if (!ts->data) {
|
||||
throw std::runtime_error("could not init rosidl message type support impl");
|
||||
}
|
||||
if (ts->typesupport_identifier != rosidl_get_dynamic_typesupport_identifier()) {
|
||||
throw std::runtime_error("rosidl message type support is of the wrong type");
|
||||
}
|
||||
|
||||
// NOTE(methylDragon): Not technically const correct, but since it's a const void *,
|
||||
// we do it anyway...
|
||||
auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
|
||||
|
||||
// NOTE(methylDragon): We don't destroy the rosidl_message_type_support->data since its members
|
||||
// are managed by the passed in SharedPtr wrapper classes. We just delete it.
|
||||
rosidl_message_type_support_.reset(
|
||||
ts,
|
||||
[](rosidl_message_type_support_t * ts) -> void {
|
||||
auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
|
||||
auto allocator = rcl_get_default_allocator();
|
||||
|
||||
// These are all C allocated
|
||||
allocator.deallocate(ts_impl->type_hash, &allocator.state);
|
||||
allocator.deallocate(
|
||||
const_cast<rosidl_dynamic_message_type_support_impl_t *>(ts_impl), &allocator.state);
|
||||
allocator.deallocate(ts, &allocator.state);
|
||||
}
|
||||
);
|
||||
|
||||
manage_description_(ts_impl->type_description);
|
||||
serialization_support_ = DynamicSerializationSupport::make_shared(ts_impl->serialization_support);
|
||||
|
||||
dynamic_message_type_ = DynamicMessageType::make_shared(
|
||||
get_shared_dynamic_serialization_support(), ts_impl->dynamic_message_type);
|
||||
|
||||
dynamic_message_ = DynamicMessage::make_shared(
|
||||
get_shared_dynamic_serialization_support(), ts_impl->dynamic_message);
|
||||
|
||||
if (!rosidl_message_type_support_) {
|
||||
throw std::runtime_error("could not init rosidl message type support.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeSupport::DynamicMessageTypeSupport(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description)
|
||||
: serialization_support_(serialization_support),
|
||||
dynamic_message_type_(nullptr),
|
||||
dynamic_message_(nullptr),
|
||||
description_(nullptr),
|
||||
rosidl_message_type_support_(nullptr)
|
||||
{
|
||||
// Check null
|
||||
if (!serialization_support) {
|
||||
throw std::runtime_error("serialization_support cannot be nullptr.");
|
||||
}
|
||||
|
||||
rosidl_message_type_support_t * ts = nullptr;
|
||||
|
||||
auto type_hash = std::make_unique<rosidl_type_hash_t>();
|
||||
rcutils_ret_t hash_ret = rcl_calculate_type_hash(
|
||||
// TODO(methylDragon): Swap this out with the conversion function when it is ready
|
||||
reinterpret_cast<const type_description_interfaces__msg__TypeDescription *>(&description),
|
||||
type_hash.get());
|
||||
if (hash_ret != RCL_RET_OK || !type_hash) {
|
||||
throw std::runtime_error("failed to get type hash");
|
||||
}
|
||||
|
||||
rcutils_ret_t ret = rosidl_dynamic_message_type_support_handle_create(
|
||||
serialization_support->get_rosidl_serialization_support(),
|
||||
type_hash.get(), // type_hash
|
||||
&description, // type_description
|
||||
nullptr, // type_description_sources (not implemented for dynamic types)
|
||||
&ts);
|
||||
if (ret != RCUTILS_RET_OK || !ts) {
|
||||
throw std::runtime_error("could not init rosidl message type support");
|
||||
}
|
||||
if (!ts->data) {
|
||||
throw std::runtime_error("could not init rosidl message type support impl");
|
||||
}
|
||||
if (ts->typesupport_identifier != rosidl_get_dynamic_typesupport_identifier()) {
|
||||
throw std::runtime_error("rosidl message type support is of the wrong type");
|
||||
}
|
||||
|
||||
auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
|
||||
|
||||
// NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members
|
||||
// are managed by the passed in SharedPtr wrapper classes. We just delete it.
|
||||
rosidl_message_type_support_.reset(
|
||||
ts,
|
||||
[](rosidl_message_type_support_t * ts) -> void {
|
||||
auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
|
||||
|
||||
// These are allocated with new
|
||||
delete ts_impl->type_hash; // Only because we should've allocated it here
|
||||
delete ts_impl;
|
||||
delete ts;
|
||||
}
|
||||
);
|
||||
manage_description_(ts_impl->type_description);
|
||||
|
||||
dynamic_message_type_ = DynamicMessageType::make_shared(
|
||||
get_shared_dynamic_serialization_support(), ts_impl->dynamic_message_type);
|
||||
|
||||
dynamic_message_ = DynamicMessage::make_shared(
|
||||
get_shared_dynamic_serialization_support(), ts_impl->dynamic_message);
|
||||
|
||||
if (!rosidl_message_type_support_) {
|
||||
throw std::runtime_error("could not init rosidl message type support.");
|
||||
}
|
||||
|
||||
type_hash.release();
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeSupport::DynamicMessageTypeSupport(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
DynamicMessageType::SharedPtr dynamic_message_type,
|
||||
DynamicMessage::SharedPtr dynamic_message,
|
||||
const rosidl_runtime_c__type_description__TypeDescription & description)
|
||||
: serialization_support_(serialization_support),
|
||||
dynamic_message_type_(dynamic_message_type),
|
||||
dynamic_message_(dynamic_message),
|
||||
description_(nullptr),
|
||||
rosidl_message_type_support_(nullptr)
|
||||
{
|
||||
// Check null
|
||||
if (!serialization_support) {
|
||||
throw std::runtime_error("serialization_support cannot be nullptr.");
|
||||
}
|
||||
if (!dynamic_message_type) {
|
||||
throw std::runtime_error("dynamic_message_type cannot be nullptr.");
|
||||
}
|
||||
if (!dynamic_message) {
|
||||
throw std::runtime_error("dynamic_message cannot be nullptr.");
|
||||
}
|
||||
|
||||
description_.reset(
|
||||
new rosidl_runtime_c__type_description__TypeDescription(),
|
||||
[](rosidl_runtime_c__type_description__TypeDescription * description) -> void {
|
||||
rosidl_runtime_c__type_description__TypeDescription__destroy(description);
|
||||
});
|
||||
if (!description_) {
|
||||
throw std::runtime_error("could not init type description.");
|
||||
}
|
||||
if (!rosidl_runtime_c__type_description__TypeDescription__copy(
|
||||
&description,
|
||||
description_.get()))
|
||||
{
|
||||
throw std::runtime_error("could not copy type description.");
|
||||
}
|
||||
|
||||
// Check identifiers
|
||||
if (serialization_support->get_library_identifier() !=
|
||||
dynamic_message_type->get_library_identifier())
|
||||
{
|
||||
throw std::runtime_error(
|
||||
"serialization support library identifier does not match "
|
||||
"dynamic message type library identifier.");
|
||||
}
|
||||
if (dynamic_message_type->get_library_identifier() != dynamic_message->get_library_identifier()) {
|
||||
throw std::runtime_error(
|
||||
"dynamic message type library identifier does not match "
|
||||
"dynamic message library identifier.");
|
||||
}
|
||||
|
||||
// Check pointers
|
||||
/* *INDENT-OFF* */
|
||||
if (serialization_support->get_rosidl_serialization_support() !=
|
||||
dynamic_message_type
|
||||
->get_shared_dynamic_serialization_support()
|
||||
->get_rosidl_serialization_support())
|
||||
{
|
||||
throw std::runtime_error("serialization support pointer does not match dynamic message type's");
|
||||
}
|
||||
if (dynamic_message_type
|
||||
->get_shared_dynamic_serialization_support()
|
||||
->get_rosidl_serialization_support() !=
|
||||
dynamic_message_type
|
||||
->get_shared_dynamic_serialization_support()
|
||||
->get_rosidl_serialization_support())
|
||||
{
|
||||
throw std::runtime_error("serialization support does not match pointer dynamic message type's");
|
||||
}
|
||||
/* *INDENT-ON* */
|
||||
|
||||
init_rosidl_message_type_support_(
|
||||
serialization_support_, dynamic_message_type_, dynamic_message_, description_.get());
|
||||
if (!rosidl_message_type_support_) {
|
||||
throw std::runtime_error("could not init rosidl message type support.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageTypeSupport::~DynamicMessageTypeSupport() {}
|
||||
|
||||
|
||||
void
|
||||
DynamicMessageTypeSupport::manage_description_(
|
||||
rosidl_runtime_c__type_description__TypeDescription * description)
|
||||
{
|
||||
if (!description) {
|
||||
throw std::runtime_error("description cannot be nullptr");
|
||||
}
|
||||
description_.reset(
|
||||
description,
|
||||
[](rosidl_runtime_c__type_description__TypeDescription * description) -> void {
|
||||
rosidl_runtime_c__type_description__TypeDescription__destroy(description);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
// This looks like rmw_`dynamic_message_type_support_handle_create()`, but instead just aggregates
|
||||
// already initialized objects
|
||||
void
|
||||
DynamicMessageTypeSupport::init_rosidl_message_type_support_(
|
||||
DynamicSerializationSupport::SharedPtr serialization_support,
|
||||
DynamicMessageType::SharedPtr dynamic_message_type,
|
||||
DynamicMessage::SharedPtr dynamic_message,
|
||||
rosidl_runtime_c__type_description__TypeDescription * description)
|
||||
{
|
||||
bool middleware_supports_type_discovery = rmw_feature_supported(
|
||||
RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY);
|
||||
|
||||
if (!middleware_supports_type_discovery && !description) {
|
||||
throw std::runtime_error(
|
||||
"Middleware does not support type discovery! Deferred dynamic type"
|
||||
"message type support will never be populated! You must provide a type "
|
||||
"description.");
|
||||
return;
|
||||
}
|
||||
|
||||
auto type_hash = std::make_unique<rosidl_type_hash_t>();
|
||||
rcutils_ret_t hash_ret = rcl_calculate_type_hash(
|
||||
// TODO(methylDragon): Swap this out with the conversion function when it is ready
|
||||
// from https://github.com/ros2/rcl/pull/1052
|
||||
reinterpret_cast<type_description_interfaces__msg__TypeDescription *>(description),
|
||||
type_hash.get());
|
||||
if (hash_ret != RCL_RET_OK || !type_hash) {
|
||||
throw std::runtime_error("failed to get type hash");
|
||||
}
|
||||
|
||||
rosidl_dynamic_message_type_support_impl_t * ts_impl =
|
||||
new rosidl_dynamic_message_type_support_impl_t {
|
||||
type_hash.get(), // type_hash
|
||||
description, // type_description
|
||||
nullptr, // NOTE(methylDragon): Not supported for now // type_description_sources
|
||||
serialization_support->get_rosidl_serialization_support(), // serialization_support
|
||||
dynamic_message_type->get_rosidl_dynamic_type(), // dynamic_message_type
|
||||
dynamic_message->get_rosidl_dynamic_data() // dynamic_message
|
||||
};
|
||||
if (!ts_impl) {
|
||||
throw std::runtime_error(
|
||||
"Could not allocate rosidl_dynamic_message_type_support_impl_t struct");
|
||||
return;
|
||||
}
|
||||
|
||||
// NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members
|
||||
// are managed by the passed in SharedPtr wrapper classes. We just delete it.
|
||||
rosidl_message_type_support_.reset(
|
||||
new rosidl_message_type_support_t{
|
||||
rosidl_get_dynamic_typesupport_identifier(), // typesupport_identifier
|
||||
ts_impl, // data
|
||||
get_message_typesupport_handle_function, // func
|
||||
// get_type_hash_func
|
||||
rosidl_get_dynamic_message_type_support_type_hash_function,
|
||||
// get_type_description_func
|
||||
rosidl_get_dynamic_message_type_support_type_description_function,
|
||||
// get_type_description_sources_func
|
||||
rosidl_get_dynamic_message_type_support_type_description_sources_function
|
||||
},
|
||||
[](rosidl_message_type_support_t * ts) -> void {
|
||||
auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
|
||||
auto allocator = rcl_get_default_allocator();
|
||||
// Only because we should've allocated it here (also it's C allocated)
|
||||
allocator.deallocate(ts_impl->type_hash, &allocator.state);
|
||||
delete ts_impl;
|
||||
}
|
||||
);
|
||||
|
||||
if (!rosidl_message_type_support_) {
|
||||
throw std::runtime_error("Could not allocate rosidl_message_type_support_t struct");
|
||||
delete ts_impl;
|
||||
return;
|
||||
}
|
||||
|
||||
type_hash.release();
|
||||
}
|
||||
|
||||
|
||||
// GETTERS =========================================================================================
|
||||
const std::string
|
||||
DynamicMessageTypeSupport::get_library_identifier() const
|
||||
{
|
||||
return serialization_support_->get_library_identifier();
|
||||
}
|
||||
|
||||
|
||||
rosidl_message_type_support_t *
|
||||
DynamicMessageTypeSupport::get_rosidl_message_type_support()
|
||||
{
|
||||
return rosidl_message_type_support_.get();
|
||||
}
|
||||
|
||||
|
||||
const rosidl_message_type_support_t *
|
||||
DynamicMessageTypeSupport::get_rosidl_message_type_support() const
|
||||
{
|
||||
return rosidl_message_type_support_.get();
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<rosidl_message_type_support_t>
|
||||
DynamicMessageTypeSupport::get_shared_rosidl_message_type_support()
|
||||
{
|
||||
return rosidl_message_type_support_;
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<const rosidl_message_type_support_t>
|
||||
DynamicMessageTypeSupport::get_shared_rosidl_message_type_support() const
|
||||
{
|
||||
return rosidl_message_type_support_;
|
||||
}
|
||||
|
||||
|
||||
rosidl_runtime_c__type_description__TypeDescription *
|
||||
DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description()
|
||||
{
|
||||
return description_.get();
|
||||
}
|
||||
|
||||
|
||||
const rosidl_runtime_c__type_description__TypeDescription *
|
||||
DynamicMessageTypeSupport::get_rosidl_runtime_c_type_description() const
|
||||
{
|
||||
return description_.get();
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<rosidl_runtime_c__type_description__TypeDescription>
|
||||
DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description()
|
||||
{
|
||||
return description_;
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<const rosidl_runtime_c__type_description__TypeDescription>
|
||||
DynamicMessageTypeSupport::get_shared_rosidl_runtime_c_type_description() const
|
||||
{
|
||||
return description_;
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::SharedPtr
|
||||
DynamicMessageTypeSupport::get_shared_dynamic_serialization_support()
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::ConstSharedPtr
|
||||
DynamicMessageTypeSupport::get_shared_dynamic_serialization_support() const
|
||||
{
|
||||
return serialization_support_;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::SharedPtr
|
||||
DynamicMessageTypeSupport::get_shared_dynamic_message_type()
|
||||
{
|
||||
return dynamic_message_type_;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessageType::ConstSharedPtr
|
||||
DynamicMessageTypeSupport::get_shared_dynamic_message_type() const
|
||||
{
|
||||
return dynamic_message_type_;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::SharedPtr
|
||||
DynamicMessageTypeSupport::get_shared_dynamic_message()
|
||||
{
|
||||
return dynamic_message_;
|
||||
}
|
||||
|
||||
|
||||
DynamicMessage::ConstSharedPtr
|
||||
DynamicMessageTypeSupport::get_shared_dynamic_message() const
|
||||
{
|
||||
return dynamic_message_;
|
||||
}
|
||||
|
||||
@@ -12,35 +12,125 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rcutils/logging_macros.h>
|
||||
#include <rmw/dynamic_message_type_support.h>
|
||||
#include <rmw/ret_types.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
#include "rmw/dynamic_message_type_support.h"
|
||||
#include "rmw/ret_types.h"
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
// CONSTRUCTION ====================================================================================
|
||||
DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator)
|
||||
: DynamicSerializationSupport::DynamicSerializationSupport("", allocator)
|
||||
{
|
||||
throw std::runtime_error("Unimplemented");
|
||||
}
|
||||
DynamicSerializationSupport::DynamicSerializationSupport()
|
||||
: DynamicSerializationSupport::DynamicSerializationSupport("") {}
|
||||
|
||||
DynamicSerializationSupport::DynamicSerializationSupport(
|
||||
const std::string & /*serialization_library_name*/,
|
||||
rcl_allocator_t /*allocator*/)
|
||||
: rosidl_serialization_support_(
|
||||
rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())
|
||||
const std::string & serialization_library_name)
|
||||
: rosidl_serialization_support_(nullptr)
|
||||
{
|
||||
throw std::runtime_error("Unimplemented");
|
||||
rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support = nullptr;
|
||||
rmw_ret_t ret = RMW_RET_ERROR;
|
||||
|
||||
if (serialization_library_name.empty()) {
|
||||
ret = rmw_get_serialization_support(NULL, &rosidl_serialization_support);
|
||||
} else {
|
||||
ret = rmw_get_serialization_support(
|
||||
serialization_library_name.c_str(), &rosidl_serialization_support);
|
||||
}
|
||||
|
||||
if (ret != RMW_RET_OK || !rosidl_serialization_support) {
|
||||
throw std::runtime_error("could not create new serialization support object");
|
||||
}
|
||||
|
||||
rosidl_serialization_support_.reset(
|
||||
rosidl_serialization_support,
|
||||
// Custom deleter
|
||||
[](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void {
|
||||
rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support);
|
||||
});
|
||||
}
|
||||
|
||||
DynamicSerializationSupport::~DynamicSerializationSupport()
|
||||
{} // STUBBED
|
||||
|
||||
DynamicSerializationSupport::DynamicSerializationSupport(
|
||||
rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support)
|
||||
: rosidl_serialization_support_(nullptr)
|
||||
{
|
||||
if (!rosidl_serialization_support) {
|
||||
throw std::runtime_error("serialization support cannot be nullptr!");
|
||||
}
|
||||
|
||||
// Custom deleter
|
||||
rosidl_serialization_support_.reset(
|
||||
rosidl_serialization_support,
|
||||
[](rosidl_dynamic_typesupport_serialization_support_t * rosidl_serialization_support) -> void {
|
||||
rosidl_dynamic_typesupport_serialization_support_destroy(rosidl_serialization_support);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::DynamicSerializationSupport(
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t> rosidl_serialization_support)
|
||||
: rosidl_serialization_support_(rosidl_serialization_support) {}
|
||||
|
||||
|
||||
DynamicSerializationSupport::DynamicSerializationSupport(
|
||||
DynamicSerializationSupport && other) noexcept
|
||||
: rosidl_serialization_support_(std::exchange(other.rosidl_serialization_support_, nullptr)) {}
|
||||
|
||||
|
||||
DynamicSerializationSupport &
|
||||
DynamicSerializationSupport::operator=(DynamicSerializationSupport && other) noexcept
|
||||
{
|
||||
std::swap(rosidl_serialization_support_, other.rosidl_serialization_support_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
DynamicSerializationSupport::~DynamicSerializationSupport() {}
|
||||
|
||||
|
||||
// GETTERS =========================================================================================
|
||||
const std::string
|
||||
DynamicSerializationSupport::get_library_identifier() const
|
||||
{
|
||||
return std::string(
|
||||
rosidl_dynamic_typesupport_serialization_support_get_library_identifier(
|
||||
rosidl_serialization_support_.get()));
|
||||
}
|
||||
|
||||
|
||||
rosidl_dynamic_typesupport_serialization_support_t *
|
||||
DynamicSerializationSupport::get_rosidl_serialization_support()
|
||||
{
|
||||
return rosidl_serialization_support_.get();
|
||||
}
|
||||
|
||||
|
||||
const rosidl_dynamic_typesupport_serialization_support_t *
|
||||
DynamicSerializationSupport::get_rosidl_serialization_support() const
|
||||
{
|
||||
return rosidl_serialization_support_.get();
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>
|
||||
DynamicSerializationSupport::get_shared_rosidl_serialization_support()
|
||||
{
|
||||
return std::shared_ptr<rosidl_dynamic_typesupport_serialization_support_t>(
|
||||
shared_from_this(), rosidl_serialization_support_.get());
|
||||
}
|
||||
|
||||
|
||||
std::shared_ptr<const rosidl_dynamic_typesupport_serialization_support_t>
|
||||
DynamicSerializationSupport::get_shared_rosidl_serialization_support() const
|
||||
{
|
||||
return std::shared_ptr<const rosidl_dynamic_typesupport_serialization_support_t>(
|
||||
shared_from_this(), rosidl_serialization_support_.get());
|
||||
}
|
||||
|
||||
@@ -39,6 +39,13 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
|
||||
|
||||
EventHandlerBase::~EventHandlerBase()
|
||||
{
|
||||
// Since the rmw event listener holds a reference to
|
||||
// this callback, we need to clear it on destruction of this class.
|
||||
// This clearing is not needed for other rclcpp entities like pub/subs, since
|
||||
// they do own the underlying rmw entities, which are destroyed
|
||||
// on their rclcpp destructors, thus no risk of dangling pointers.
|
||||
clear_on_ready_callback();
|
||||
|
||||
if (rcl_event_fini(&event_handle_) != RCL_RET_OK) {
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rclcpp",
|
||||
|
||||
@@ -39,16 +39,16 @@
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
using rclcpp::exceptions::throw_from_rcl_error;
|
||||
using rclcpp::AnyExecutable;
|
||||
using rclcpp::Executor;
|
||||
|
||||
class rclcpp::ExecutorImplementation {};
|
||||
using rclcpp::ExecutorOptions;
|
||||
using rclcpp::FutureReturnCode;
|
||||
|
||||
Executor::Executor(const rclcpp::ExecutorOptions & options)
|
||||
: spinning(false),
|
||||
interrupt_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
|
||||
interrupt_guard_condition_(options.context),
|
||||
shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
|
||||
memory_strategy_(options.memory_strategy),
|
||||
impl_(std::make_unique<rclcpp::ExecutorImplementation>())
|
||||
memory_strategy_(options.memory_strategy)
|
||||
{
|
||||
// Store the context for later use.
|
||||
context_ = options.context;
|
||||
@@ -66,7 +66,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
|
||||
memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get());
|
||||
|
||||
// Put the executor's guard condition in
|
||||
memory_strategy_->add_guard_condition(*interrupt_guard_condition_.get());
|
||||
memory_strategy_->add_guard_condition(interrupt_guard_condition_);
|
||||
rcl_allocator_t allocator = memory_strategy_->get_allocator();
|
||||
|
||||
rcl_ret_t ret = rcl_wait_set_init(
|
||||
@@ -95,9 +95,8 @@ Executor::~Executor()
|
||||
}
|
||||
// Disassociate all nodes.
|
||||
std::for_each(
|
||||
weak_nodes_.begin(), weak_nodes_.end(),
|
||||
[](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr)
|
||||
{
|
||||
weak_nodes_.begin(), weak_nodes_.end(), []
|
||||
(rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) {
|
||||
auto shared_node_ptr = weak_node_ptr.lock();
|
||||
if (shared_node_ptr) {
|
||||
std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic();
|
||||
@@ -129,7 +128,7 @@ Executor::~Executor()
|
||||
}
|
||||
// Remove and release the sigint guard condition
|
||||
memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get());
|
||||
memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get());
|
||||
memory_strategy_->remove_guard_condition(&interrupt_guard_condition_);
|
||||
|
||||
// Remove shutdown callback handle registered to Context
|
||||
if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) {
|
||||
@@ -224,7 +223,8 @@ Executor::add_callback_group_to_map(
|
||||
weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
|
||||
|
||||
if (node_ptr->get_context()->is_valid()) {
|
||||
auto callback_group_guard_condition = group_ptr->get_notify_guard_condition();
|
||||
auto callback_group_guard_condition =
|
||||
group_ptr->get_notify_guard_condition(node_ptr->get_context());
|
||||
weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
|
||||
// Add the callback_group's notify condition to the guard condition handles
|
||||
memory_strategy_->add_guard_condition(*callback_group_guard_condition);
|
||||
@@ -233,7 +233,7 @@ Executor::add_callback_group_to_map(
|
||||
if (notify) {
|
||||
// Interrupt waiting to handle new node
|
||||
try {
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
} catch (const rclcpp::exceptions::RCLError & ex) {
|
||||
throw std::runtime_error(
|
||||
std::string(
|
||||
@@ -281,10 +281,10 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
|
||||
}
|
||||
});
|
||||
|
||||
const auto gc = node_ptr->get_shared_notify_guard_condition();
|
||||
weak_nodes_to_guard_conditions_[node_ptr] = gc.get();
|
||||
const auto & gc = node_ptr->get_notify_guard_condition();
|
||||
weak_nodes_to_guard_conditions_[node_ptr] = &gc;
|
||||
// Add the node's notify condition to the guard condition handles
|
||||
memory_strategy_->add_guard_condition(*gc);
|
||||
memory_strategy_->add_guard_condition(gc);
|
||||
weak_nodes_.push_back(node_ptr);
|
||||
}
|
||||
|
||||
@@ -321,7 +321,7 @@ Executor::remove_callback_group_from_map(
|
||||
|
||||
if (notify) {
|
||||
try {
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
} catch (const rclcpp::exceptions::RCLError & ex) {
|
||||
throw std::runtime_error(
|
||||
std::string(
|
||||
@@ -389,7 +389,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
|
||||
}
|
||||
}
|
||||
|
||||
memory_strategy_->remove_guard_condition(node_ptr->get_shared_notify_guard_condition().get());
|
||||
memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
|
||||
weak_nodes_to_guard_conditions_.erase(node_ptr);
|
||||
|
||||
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
|
||||
@@ -432,22 +432,6 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration)
|
||||
return this->spin_some_impl(max_duration, false);
|
||||
}
|
||||
|
||||
void
|
||||
Executor::spin_node_all(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
|
||||
std::chrono::nanoseconds max_duration)
|
||||
{
|
||||
this->add_node(node, false);
|
||||
spin_all(max_duration);
|
||||
this->remove_node(node, false);
|
||||
}
|
||||
|
||||
void
|
||||
Executor::spin_node_all(std::shared_ptr<rclcpp::Node> node, std::chrono::nanoseconds max_duration)
|
||||
{
|
||||
this->spin_node_all(node->get_node_base_interface(), max_duration);
|
||||
}
|
||||
|
||||
void Executor::spin_all(std::chrono::nanoseconds max_duration)
|
||||
{
|
||||
if (max_duration < 0ns) {
|
||||
@@ -518,7 +502,7 @@ Executor::cancel()
|
||||
{
|
||||
spinning.store(false);
|
||||
try {
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
} catch (const rclcpp::exceptions::RCLError & ex) {
|
||||
throw std::runtime_error(
|
||||
std::string("Failed to trigger guard condition in cancel: ") + ex.what());
|
||||
@@ -542,13 +526,13 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
|
||||
return;
|
||||
}
|
||||
if (any_exec.timer) {
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_executor_execute,
|
||||
static_cast<const void *>(any_exec.timer->get_timer_handle().get()));
|
||||
execute_timer(any_exec.timer);
|
||||
}
|
||||
if (any_exec.subscription) {
|
||||
TRACETOOLS_TRACEPOINT(
|
||||
TRACEPOINT(
|
||||
rclcpp_executor_execute,
|
||||
static_cast<const void *>(any_exec.subscription->get_subscription_handle().get()));
|
||||
execute_subscription(any_exec.subscription);
|
||||
@@ -567,7 +551,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
|
||||
// Wake the wait, because it may need to be recalculated or work that
|
||||
// was previously blocked is now available.
|
||||
try {
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
} catch (const rclcpp::exceptions::RCLError & ex) {
|
||||
throw std::runtime_error(
|
||||
std::string(
|
||||
@@ -620,9 +604,9 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
|
||||
rclcpp::MessageInfo message_info;
|
||||
message_info.get_rmw_message_info().from_intra_process = false;
|
||||
|
||||
switch (subscription->get_delivered_message_kind()) {
|
||||
// Deliver ROS message
|
||||
case rclcpp::DeliveredMessageKind::ROS_MESSAGE:
|
||||
switch (subscription->get_subscription_type()) {
|
||||
// Take ROS message
|
||||
case rclcpp::SubscriptionType::ROS_MESSAGE:
|
||||
{
|
||||
if (subscription->can_loan_messages()) {
|
||||
// This is the case where a loaned message is taken from the middleware via
|
||||
@@ -670,18 +654,13 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
|
||||
subscription->get_topic_name(),
|
||||
[&]() {return subscription->take_type_erased(message.get(), message_info);},
|
||||
[&]() {subscription->handle_message(message, message_info);});
|
||||
// TODO(clalancette): In the case that the user is using the MessageMemoryPool,
|
||||
// and they take a shared_ptr reference to the message in the callback, this can
|
||||
// inadvertently return the message to the pool when the user is still using it.
|
||||
// This is a bug that needs to be fixed in the pool, and we should probably have
|
||||
// a custom deleter for the message that actually does the return_message().
|
||||
subscription->return_message(message);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// Deliver serialized message
|
||||
case rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE:
|
||||
// Take serialized message
|
||||
case rclcpp::SubscriptionType::SERIALIZED_MESSAGE:
|
||||
{
|
||||
// This is the case where a copy of the serialized message is taken from
|
||||
// the middleware via inter-process communication.
|
||||
@@ -700,15 +679,53 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription)
|
||||
}
|
||||
|
||||
// DYNAMIC SUBSCRIPTION ========================================================================
|
||||
// Deliver dynamic message
|
||||
case rclcpp::DeliveredMessageKind::DYNAMIC_MESSAGE:
|
||||
// If a subscription is dynamic, then it will use its serialization-specific dynamic data.
|
||||
//
|
||||
// Two cases:
|
||||
// - Dynamic type subscription using dynamic type stored in its own internal type support struct
|
||||
// - Non-dynamic type subscription with no stored dynamic type
|
||||
// - Subscriptions of this type must be able to lookup the local message description to
|
||||
// generate a dynamic type at runtime!
|
||||
// - TODO(methylDragon): I won't be handling this case yet
|
||||
|
||||
// Take dynamic message directly from the middleware
|
||||
case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT:
|
||||
{
|
||||
throw std::runtime_error("Unimplemented");
|
||||
DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message();
|
||||
take_and_do_error_handling(
|
||||
"taking a dynamic message from topic",
|
||||
subscription->get_topic_name(),
|
||||
// This modifies the stored dynamic data in the DynamicMessage in-place
|
||||
[&]() {return subscription->take_dynamic_message(*dynamic_message, message_info);},
|
||||
[&]() {subscription->handle_dynamic_message(dynamic_message, message_info);});
|
||||
subscription->return_dynamic_message(dynamic_message);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
// Take serialized and then convert to dynamic message
|
||||
case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED:
|
||||
{
|
||||
throw std::runtime_error("Delivered message kind is not supported");
|
||||
std::shared_ptr<SerializedMessage> serialized_msg =
|
||||
subscription->create_serialized_message();
|
||||
|
||||
// NOTE(methylDragon): Is this clone necessary? If I'm following the pattern, it seems so.
|
||||
DynamicMessage::SharedPtr dynamic_message = subscription->create_dynamic_message();
|
||||
take_and_do_error_handling(
|
||||
"taking a serialized message from topic",
|
||||
subscription->get_topic_name(),
|
||||
[&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);},
|
||||
[&]()
|
||||
{
|
||||
bool ret = dynamic_message->deserialize(&serialized_msg->get_rcl_serialized_message());
|
||||
if (!ret) {
|
||||
throw_from_rcl_error(ret, "Couldn't convert serialized message to dynamic data!");
|
||||
}
|
||||
subscription->handle_dynamic_message(dynamic_message, message_info);
|
||||
}
|
||||
);
|
||||
subscription->return_serialized_message(serialized_msg);
|
||||
subscription->return_dynamic_message(dynamic_message);
|
||||
break;
|
||||
}
|
||||
}
|
||||
return;
|
||||
@@ -748,7 +765,7 @@ Executor::execute_client(
|
||||
void
|
||||
Executor::wait_for_work(std::chrono::nanoseconds timeout)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
|
||||
TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
|
||||
@@ -904,7 +921,7 @@ Executor::get_next_ready_executable_from_map(
|
||||
const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
|
||||
weak_groups_to_nodes)
|
||||
{
|
||||
TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready);
|
||||
TRACEPOINT(rclcpp_executor_get_next_ready);
|
||||
bool success = false;
|
||||
std::lock_guard<std::mutex> guard{mutex_};
|
||||
// Check the timers to see if there are any that are ready
|
||||
|
||||
@@ -14,21 +14,6 @@
|
||||
|
||||
#include "rclcpp/executors.hpp"
|
||||
|
||||
void
|
||||
rclcpp::spin_all(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
std::chrono::nanoseconds max_duration)
|
||||
{
|
||||
rclcpp::executors::SingleThreadedExecutor exec;
|
||||
exec.spin_node_all(node_ptr, max_duration);
|
||||
}
|
||||
|
||||
void
|
||||
rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration)
|
||||
{
|
||||
rclcpp::spin_all(node_ptr->get_node_base_interface(), max_duration);
|
||||
}
|
||||
|
||||
void
|
||||
rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
|
||||
@@ -1,230 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp/executors/executor_entities_collection.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
bool ExecutorEntitiesCollection::empty() const
|
||||
{
|
||||
return
|
||||
subscriptions.empty() &&
|
||||
timers.empty() &&
|
||||
guard_conditions.empty() &&
|
||||
clients.empty() &&
|
||||
services.empty() &&
|
||||
waitables.empty();
|
||||
}
|
||||
|
||||
void ExecutorEntitiesCollection::clear()
|
||||
{
|
||||
subscriptions.clear();
|
||||
timers.clear();
|
||||
guard_conditions.clear();
|
||||
clients.clear();
|
||||
services.clear();
|
||||
waitables.clear();
|
||||
}
|
||||
|
||||
void
|
||||
build_entities_collection(
|
||||
const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
|
||||
ExecutorEntitiesCollection & collection)
|
||||
{
|
||||
collection.clear();
|
||||
|
||||
for (auto weak_group_ptr : callback_groups) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (!group_ptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (group_ptr->can_be_taken_from().load()) {
|
||||
group_ptr->collect_all_ptrs(
|
||||
[&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
|
||||
collection.subscriptions.insert(
|
||||
{
|
||||
subscription->get_subscription_handle().get(),
|
||||
{subscription, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) {
|
||||
collection.services.insert(
|
||||
{
|
||||
service->get_service_handle().get(),
|
||||
{service, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) {
|
||||
collection.clients.insert(
|
||||
{
|
||||
client->get_client_handle().get(),
|
||||
{client, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) {
|
||||
collection.timers.insert(
|
||||
{
|
||||
timer->get_timer_handle().get(),
|
||||
{timer, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) {
|
||||
collection.waitables.insert(
|
||||
{
|
||||
waitable.get(),
|
||||
{waitable, weak_group_ptr}
|
||||
});
|
||||
}
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
size_t
|
||||
ready_executables(
|
||||
const ExecutorEntitiesCollection & collection,
|
||||
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
|
||||
std::deque<rclcpp::AnyExecutable> & executables
|
||||
)
|
||||
{
|
||||
size_t added = 0;
|
||||
if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
|
||||
return added;
|
||||
}
|
||||
auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
|
||||
|
||||
// Cache shared pointers to groups to avoid extra work re-locking them
|
||||
std::map<rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::CallbackGroup::SharedPtr,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>> group_map;
|
||||
|
||||
auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr)
|
||||
{
|
||||
if (group_map.count(weak_cbg_ptr) == 0) {
|
||||
group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()});
|
||||
}
|
||||
return group_map.find(weak_cbg_ptr)->second;
|
||||
};
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) {
|
||||
if (nullptr == rcl_wait_set.timers[ii]) {continue;}
|
||||
auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]);
|
||||
if (entity_iter != collection.timers.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
if (!entity->call()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.timer = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) {
|
||||
if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;}
|
||||
auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]);
|
||||
if (entity_iter != collection.subscriptions.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.subscription = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) {
|
||||
if (nullptr == rcl_wait_set.services[ii]) {continue;}
|
||||
auto entity_iter = collection.services.find(rcl_wait_set.services[ii]);
|
||||
if (entity_iter != collection.services.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.service = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) {
|
||||
if (nullptr == rcl_wait_set.clients[ii]) {continue;}
|
||||
auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]);
|
||||
if (entity_iter != collection.clients.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.client = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & [handle, entry] : collection.waitables) {
|
||||
auto waitable = entry.entity.lock();
|
||||
if (!waitable) {
|
||||
continue;
|
||||
}
|
||||
if (!waitable->is_ready(&rcl_wait_set)) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entry.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.waitable = waitable;
|
||||
exec.callback_group = group_info;
|
||||
exec.data = waitable->take_data();
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
|
||||
return added;
|
||||
}
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
@@ -1,416 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <set>
|
||||
|
||||
#include "rclcpp/executors/executor_entities_collector.hpp"
|
||||
#include "rclcpp/executors/executor_notify_waitable.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
ExecutorEntitiesCollector::ExecutorEntitiesCollector(
|
||||
std::shared_ptr<ExecutorNotifyWaitable> notify_waitable)
|
||||
: notify_waitable_(notify_waitable)
|
||||
{
|
||||
}
|
||||
|
||||
ExecutorEntitiesCollector::~ExecutorEntitiesCollector()
|
||||
{
|
||||
for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end(); ) {
|
||||
weak_node_it = remove_weak_node(weak_node_it);
|
||||
}
|
||||
|
||||
for (auto weak_group_it = automatically_added_groups_.begin();
|
||||
weak_group_it != automatically_added_groups_.end(); )
|
||||
{
|
||||
weak_group_it = remove_weak_callback_group(weak_group_it, automatically_added_groups_);
|
||||
}
|
||||
|
||||
for (auto weak_group_it = manually_added_groups_.begin();
|
||||
weak_group_it != manually_added_groups_.end(); )
|
||||
{
|
||||
weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_);
|
||||
}
|
||||
|
||||
for (auto weak_node_ptr : pending_added_nodes_) {
|
||||
auto node_ptr = weak_node_ptr.lock();
|
||||
if (node_ptr) {
|
||||
node_ptr->get_associated_with_executor_atomic().store(false);
|
||||
}
|
||||
}
|
||||
pending_added_nodes_.clear();
|
||||
pending_removed_nodes_.clear();
|
||||
|
||||
for (auto weak_group_ptr : pending_manually_added_groups_) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (group_ptr) {
|
||||
group_ptr->get_associated_with_executor_atomic().store(false);
|
||||
}
|
||||
}
|
||||
pending_manually_added_groups_.clear();
|
||||
pending_manually_removed_groups_.clear();
|
||||
}
|
||||
|
||||
bool
|
||||
ExecutorEntitiesCollector::has_pending() const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return pending_manually_added_groups_.size() != 0 ||
|
||||
pending_manually_removed_groups_.size() != 0 ||
|
||||
pending_added_nodes_.size() != 0 ||
|
||||
pending_removed_nodes_.size() != 0;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
// 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(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' has already been added to an executor.");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = weak_nodes_.count(node_ptr) != 0;
|
||||
bool add_queued = pending_added_nodes_.count(node_ptr) != 0;
|
||||
bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0;
|
||||
|
||||
if ((associated || add_queued) && !remove_queued) {
|
||||
throw std::runtime_error(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' has already been added to this executor.");
|
||||
}
|
||||
|
||||
this->pending_added_nodes_.insert(node_ptr);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
if (!node_ptr->get_associated_with_executor_atomic().load()) {
|
||||
throw std::runtime_error(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' needs to be associated with an executor.");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = weak_nodes_.count(node_ptr) != 0;
|
||||
bool add_queued = pending_added_nodes_.count(node_ptr) != 0;
|
||||
bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0;
|
||||
|
||||
if (!(associated || add_queued) || remove_queued) {
|
||||
throw std::runtime_error(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' needs to be associated with this executor.");
|
||||
}
|
||||
|
||||
this->pending_removed_nodes_.insert(node_ptr);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
|
||||
if (has_executor.exchange(true)) {
|
||||
throw std::runtime_error("Callback group has already been added to an executor.");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = manually_added_groups_.count(group_ptr) != 0;
|
||||
bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0;
|
||||
bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0;
|
||||
|
||||
if ((associated || add_queued) && !remove_queued) {
|
||||
throw std::runtime_error("Callback group has already been added to this executor.");
|
||||
}
|
||||
|
||||
this->pending_manually_added_groups_.insert(group_ptr);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
if (!group_ptr->get_associated_with_executor_atomic().load()) {
|
||||
throw std::runtime_error("Callback group needs to be associated with an executor.");
|
||||
}
|
||||
/**
|
||||
* TODO(mjcarroll): The callback groups, being created by a node, should never outlive
|
||||
* the node. Since we haven't historically enforced this, turning this on may cause
|
||||
* previously-functional code to fail.
|
||||
* Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node),
|
||||
* when we can guarantee node/group lifetimes.
|
||||
if (!group_ptr->has_valid_node()) {
|
||||
throw std::runtime_error("Node must not be deleted before its callback group(s).");
|
||||
}
|
||||
*/
|
||||
|
||||
auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = manually_added_groups_.count(group_ptr) != 0;
|
||||
bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0;
|
||||
bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0;
|
||||
|
||||
if (!(associated || add_queued) || remove_queued) {
|
||||
throw std::runtime_error("Callback group needs to be associated with this executor.");
|
||||
}
|
||||
|
||||
this->pending_manually_removed_groups_.insert(group_ptr);
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
ExecutorEntitiesCollector::get_all_callback_groups() const
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
for (const auto & group_ptr : manually_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
for (auto const & group_ptr : automatically_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
ExecutorEntitiesCollector::get_manually_added_callback_groups() const
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
for (const auto & group_ptr : manually_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
ExecutorEntitiesCollector::get_automatically_added_callback_groups() const
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
for (auto const & group_ptr : automatically_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::update_collections()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
this->process_queues();
|
||||
this->add_automatically_associated_callback_groups(this->weak_nodes_);
|
||||
this->prune_invalid_nodes_and_groups();
|
||||
}
|
||||
|
||||
ExecutorEntitiesCollector::NodeCollection::iterator
|
||||
ExecutorEntitiesCollector::remove_weak_node(NodeCollection::iterator weak_node)
|
||||
{
|
||||
// Disassociate the guard condition from the executor notify waitable
|
||||
auto guard_condition_it = weak_nodes_to_guard_conditions_.find(*weak_node);
|
||||
if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) {
|
||||
this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
|
||||
weak_nodes_to_guard_conditions_.erase(guard_condition_it);
|
||||
}
|
||||
|
||||
// Mark the node as disassociated (if the node is still valid)
|
||||
auto node_ptr = weak_node->lock();
|
||||
if (node_ptr) {
|
||||
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
}
|
||||
|
||||
// Remove the node from tracked nodes
|
||||
return weak_nodes_.erase(weak_node);
|
||||
}
|
||||
|
||||
ExecutorEntitiesCollector::CallbackGroupCollection::iterator
|
||||
ExecutorEntitiesCollector::remove_weak_callback_group(
|
||||
CallbackGroupCollection::iterator weak_group_it,
|
||||
CallbackGroupCollection & collection
|
||||
)
|
||||
{
|
||||
// Disassociate the guard condition from the executor notify waitable
|
||||
auto guard_condition_it = weak_groups_to_guard_conditions_.find(*weak_group_it);
|
||||
if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
|
||||
this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
|
||||
weak_groups_to_guard_conditions_.erase(guard_condition_it);
|
||||
}
|
||||
|
||||
// Mark the node as disassociated (if the group is still valid)
|
||||
auto group_ptr = weak_group_it->lock();
|
||||
if (group_ptr) {
|
||||
/**
|
||||
* TODO(mjcarroll): The callback groups, being created by a node, should never outlive
|
||||
* the node. Since we haven't historically enforced this, turning this on may cause
|
||||
* previously-functional code to fail.
|
||||
* Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node),
|
||||
* when we can guarantee node/group lifetimes.
|
||||
if (!group_ptr->has_valid_node()) {
|
||||
throw std::runtime_error("Node must not be deleted before its callback group(s).");
|
||||
}
|
||||
*/
|
||||
std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
}
|
||||
|
||||
// Remove the node from tracked nodes
|
||||
return collection.erase(weak_group_it);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_callback_group_to_collection(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
CallbackGroupCollection & collection)
|
||||
{
|
||||
auto iter = collection.insert(group_ptr);
|
||||
if (iter.second == false) {
|
||||
throw std::runtime_error("Callback group has already been added to this executor.");
|
||||
}
|
||||
|
||||
// Store node guard condition in map and add it to the notify waitable
|
||||
auto group_guard_condition = group_ptr->get_notify_guard_condition();
|
||||
weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition});
|
||||
this->notify_waitable_->add_guard_condition(group_guard_condition);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::process_queues()
|
||||
{
|
||||
for (auto weak_node_ptr : pending_added_nodes_) {
|
||||
auto node_ptr = weak_node_ptr.lock();
|
||||
if (!node_ptr) {
|
||||
continue;
|
||||
}
|
||||
weak_nodes_.insert(weak_node_ptr);
|
||||
this->add_automatically_associated_callback_groups({weak_node_ptr});
|
||||
|
||||
// Store node guard condition in map and add it to the notify waitable
|
||||
auto node_guard_condition = node_ptr->get_shared_notify_guard_condition();
|
||||
weak_nodes_to_guard_conditions_.insert({weak_node_ptr, node_guard_condition});
|
||||
this->notify_waitable_->add_guard_condition(node_guard_condition);
|
||||
}
|
||||
pending_added_nodes_.clear();
|
||||
|
||||
for (auto weak_node_ptr : pending_removed_nodes_) {
|
||||
auto node_it = weak_nodes_.find(weak_node_ptr);
|
||||
if (node_it != weak_nodes_.end()) {
|
||||
remove_weak_node(node_it);
|
||||
} else {
|
||||
throw std::runtime_error("Node needs to be associated with this executor.");
|
||||
}
|
||||
|
||||
auto node_ptr = weak_node_ptr.lock();
|
||||
if (node_ptr) {
|
||||
for (auto group_it = automatically_added_groups_.begin();
|
||||
group_it != automatically_added_groups_.end(); )
|
||||
{
|
||||
auto group_ptr = group_it->lock();
|
||||
if (node_ptr->callback_group_in_node(group_ptr)) {
|
||||
group_it = remove_weak_callback_group(group_it, automatically_added_groups_);
|
||||
} else {
|
||||
++group_it;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
pending_removed_nodes_.clear();
|
||||
|
||||
for (auto weak_group_ptr : pending_manually_added_groups_) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (group_ptr) {
|
||||
this->add_callback_group_to_collection(group_ptr, manually_added_groups_);
|
||||
}
|
||||
}
|
||||
pending_manually_added_groups_.clear();
|
||||
|
||||
for (auto weak_group_ptr : pending_manually_removed_groups_) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (group_ptr) {
|
||||
auto group_it = manually_added_groups_.find(group_ptr);
|
||||
if (group_it != manually_added_groups_.end()) {
|
||||
remove_weak_callback_group(group_it, manually_added_groups_);
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"Attempting to remove a callback group not added to this executor.");
|
||||
}
|
||||
}
|
||||
}
|
||||
pending_manually_removed_groups_.clear();
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_automatically_associated_callback_groups(
|
||||
const NodeCollection & nodes_to_check)
|
||||
{
|
||||
for (auto & weak_node : nodes_to_check) {
|
||||
auto node = weak_node.lock();
|
||||
if (node) {
|
||||
node->for_each_callback_group(
|
||||
[this, node](rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
if (!group_ptr->get_associated_with_executor_atomic().load() &&
|
||||
group_ptr->automatically_add_to_executor_with_node())
|
||||
{
|
||||
std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
|
||||
if (has_executor.exchange(true)) {
|
||||
throw std::runtime_error("Callback group has already been added to an executor.");
|
||||
}
|
||||
this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::prune_invalid_nodes_and_groups()
|
||||
{
|
||||
for (auto node_it = weak_nodes_.begin();
|
||||
node_it != weak_nodes_.end(); )
|
||||
{
|
||||
if (node_it->expired()) {
|
||||
node_it = remove_weak_node(node_it);
|
||||
} else {
|
||||
node_it++;
|
||||
}
|
||||
}
|
||||
for (auto group_it = automatically_added_groups_.begin();
|
||||
group_it != automatically_added_groups_.end(); )
|
||||
{
|
||||
if (group_it->expired()) {
|
||||
group_it = remove_weak_callback_group(group_it, automatically_added_groups_);
|
||||
} else {
|
||||
group_it++;
|
||||
}
|
||||
}
|
||||
for (auto group_it = manually_added_groups_.begin();
|
||||
group_it != manually_added_groups_.end(); )
|
||||
{
|
||||
if (group_it->expired()) {
|
||||
group_it = remove_weak_callback_group(group_it, manually_added_groups_);
|
||||
} else {
|
||||
group_it++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
@@ -1,183 +0,0 @@
|
||||
// Copyright 2023 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/executors/executor_notify_waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback)
|
||||
: execute_callback_(on_execute_callback)
|
||||
{
|
||||
}
|
||||
|
||||
ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other)
|
||||
: ExecutorNotifyWaitable(other.execute_callback_)
|
||||
{
|
||||
this->notify_guard_conditions_ = other.notify_guard_conditions_;
|
||||
}
|
||||
|
||||
ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other)
|
||||
{
|
||||
if (this != &other) {
|
||||
this->execute_callback_ = other.execute_callback_;
|
||||
this->notify_guard_conditions_ = other.notify_guard_conditions_;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
|
||||
for (auto weak_guard_condition : this->notify_guard_conditions_) {
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
if (guard_condition) {
|
||||
auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition();
|
||||
|
||||
rcl_ret_t ret = rcl_wait_set_add_guard_condition(
|
||||
wait_set,
|
||||
rcl_guard_condition, NULL);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "failed to add guard condition to wait set");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
|
||||
bool any_ready = false;
|
||||
for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) {
|
||||
auto rcl_guard_condition = wait_set->guard_conditions[ii];
|
||||
|
||||
if (nullptr == rcl_guard_condition) {
|
||||
continue;
|
||||
}
|
||||
for (auto weak_guard_condition : this->notify_guard_conditions_) {
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) {
|
||||
any_ready = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return any_ready;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::execute(std::shared_ptr<void> & data)
|
||||
{
|
||||
(void) data;
|
||||
this->execute_callback_();
|
||||
}
|
||||
|
||||
std::shared_ptr<void>
|
||||
ExecutorNotifyWaitable::take_data()
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<void>
|
||||
ExecutorNotifyWaitable::take_data_by_entity_id(size_t id)
|
||||
{
|
||||
(void) id;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::set_on_ready_callback(std::function<void(size_t, int)> callback)
|
||||
{
|
||||
// The second argument of the callback could be used to identify which guard condition
|
||||
// triggered the event.
|
||||
// We could indicate which of the guard conditions was triggered, but the executor
|
||||
// is already going to check that.
|
||||
auto gc_callback = [callback](size_t count) {
|
||||
callback(count, 0);
|
||||
};
|
||||
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
|
||||
on_ready_callback_ = gc_callback;
|
||||
for (auto weak_gc : notify_guard_conditions_) {
|
||||
auto gc = weak_gc.lock();
|
||||
if (!gc) {
|
||||
continue;
|
||||
}
|
||||
gc->set_on_trigger_callback(on_ready_callback_);
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
ExecutorNotifyWaitable::clear_on_ready_callback()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
|
||||
on_ready_callback_ = nullptr;
|
||||
for (auto weak_gc : notify_guard_conditions_) {
|
||||
auto gc = weak_gc.lock();
|
||||
if (!gc) {
|
||||
continue;
|
||||
}
|
||||
gc->set_on_trigger_callback(nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) {
|
||||
notify_guard_conditions_.insert(weak_guard_condition);
|
||||
if (on_ready_callback_) {
|
||||
guard_condition->set_on_trigger_callback(on_ready_callback_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
if (notify_guard_conditions_.count(weak_guard_condition) != 0) {
|
||||
notify_guard_conditions_.erase(weak_guard_condition);
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
// If this notify waitable doesn't have an on_ready_callback, then there's nothing to unset
|
||||
if (guard_condition && on_ready_callback_) {
|
||||
guard_condition->set_on_trigger_callback(nullptr);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
size_t
|
||||
ExecutorNotifyWaitable::get_number_of_ready_guard_conditions()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
return notify_guard_conditions_.size();
|
||||
}
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
@@ -109,8 +109,8 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
|
||||
std::lock_guard<std::mutex> guard{new_nodes_mutex_};
|
||||
for (const auto & weak_node : new_nodes_) {
|
||||
if (auto node_ptr = weak_node.lock()) {
|
||||
weak_nodes_to_guard_conditions_[node_ptr] =
|
||||
node_ptr->get_shared_notify_guard_condition().get();
|
||||
const auto & gc = node_ptr->get_notify_guard_condition();
|
||||
weak_nodes_to_guard_conditions_[node_ptr] = &gc;
|
||||
}
|
||||
}
|
||||
new_nodes_.clear();
|
||||
@@ -409,10 +409,8 @@ StaticExecutorEntitiesCollector::remove_node(
|
||||
std::for_each(
|
||||
weak_groups_to_nodes_associated_with_executor_.begin(),
|
||||
weak_groups_to_nodes_associated_with_executor_.end(),
|
||||
[&found_group_ptrs, node_ptr](
|
||||
std::pair<rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair)
|
||||
{
|
||||
[&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
|
||||
auto & weak_node_ptr = key_value_pair.second;
|
||||
auto shared_node_ptr = weak_node_ptr.lock();
|
||||
auto group_ptr = key_value_pair.first.lock();
|
||||
@@ -421,9 +419,8 @@ StaticExecutorEntitiesCollector::remove_node(
|
||||
}
|
||||
});
|
||||
std::for_each(
|
||||
found_group_ptrs.begin(), found_group_ptrs.end(),
|
||||
[this](rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
found_group_ptrs.begin(), found_group_ptrs.end(), [this]
|
||||
(rclcpp::CallbackGroup::SharedPtr group_ptr) {
|
||||
this->remove_callback_group_from_map(
|
||||
group_ptr,
|
||||
weak_groups_to_nodes_associated_with_executor_);
|
||||
|
||||
@@ -139,7 +139,7 @@ StaticSingleThreadedExecutor::add_callback_group(
|
||||
bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
|
||||
if (is_new_node && notify) {
|
||||
// Interrupt waiting to handle new node
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -150,7 +150,7 @@ StaticSingleThreadedExecutor::add_node(
|
||||
bool is_new_node = entities_collector_->add_node(node_ptr);
|
||||
if (is_new_node && notify) {
|
||||
// Interrupt waiting to handle new node
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -167,7 +167,7 @@ StaticSingleThreadedExecutor::remove_callback_group(
|
||||
bool node_removed = entities_collector_->remove_callback_group(group_ptr);
|
||||
// If the node was matched and removed, interrupt waiting
|
||||
if (node_removed && notify) {
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -181,7 +181,7 @@ StaticSingleThreadedExecutor::remove_node(
|
||||
}
|
||||
// If the node was matched and removed, interrupt waiting
|
||||
if (notify) {
|
||||
interrupt_guard_condition_->trigger();
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user