Compare commits

..

27 Commits

Author SHA1 Message Date
methylDragon
32859cdf3c Remove RMW interfaces
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-07 18:16:43 -07:00
methylDragon
30210a2c02 Remove print methods
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-07 15:43:56 -07:00
methylDragon
d871af007a Use create instead of init
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-06 21:13:58 -07:00
methylDragon
29577dab24 Move dynamic type support struct to rosidl and fix mem issues
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-05 23:12:25 -07:00
methylDragon
000f788b65 Use dynamic typesupport identifier getter
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 21:12:02 -07:00
methylDragon
ca17caf6c4 Support type hashes
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
e7e9a298c0 Add initializers for description and source getters
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
8571b28664 Support type hashes
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
7e0d063154 Lint
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
113dc01dc7 Migrate methods to use return types
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
aac28e7b9f Remove dynamic data/type aliases
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
24e533eba7 Unalias dynamic typesupport types
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
ca1fe2b41b Implement SubscriptionType enum flag
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
57900a9f44 Add fixed string support
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
363861f70c Alias dynamic_data with dynamic_message, pass desc, and build ts top-down
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
9555859691 Use new deletes for memory management
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
78a568d14a Only use general type construction and implement default values
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
9ff28439d1 Implement direct taking of dynamic messages
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
bfcc29e64b Use dynamic message type support wrappers
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
927b9de2a5 Refactor dynamic typesupport to couple serialization support to objects
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
5af57f8010 Migrate to rosidl_dynamic_typesupport and update field IDs
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
0ce81c4fed Implement first cut
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
a2484be7c3 Take ownership of description, add print, and update ser support API
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
833efe0e2d Delete redundant wrapper class definitions
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
554ad022a6 Refine wrappers
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
a3e5d0a91a Implement dynamic message type support wrapper
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
methylDragon
b8fd304546 Implement dynamic typesupport wrappers
Signed-off-by: methylDragon <methylDragon@gmail.com>
2023-04-03 15:07:43 -07:00
223 changed files with 6708 additions and 13144 deletions

View File

@@ -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>`_)

View File

@@ -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)

View File

@@ -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]

View File

@@ -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

View File

@@ -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);

View File

@@ -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);

View File

@@ -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(

View File

@@ -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.

View File

@@ -26,7 +26,6 @@
#include <unordered_set>
#include <utility>
#include <vector>
#include <stdexcept>
#include "rcl/context.h"
#include "rcl/guard_condition.h"

View File

@@ -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_

View File

@@ -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_

View File

@@ -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);

View File

@@ -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();

View File

@@ -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;
}

View File

@@ -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

View File

@@ -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_

View 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_

View File

@@ -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_

View File

@@ -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_

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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
{

View File

@@ -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

View File

@@ -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

View File

@@ -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_

View File

@@ -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_

View File

@@ -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_

View File

@@ -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

View File

@@ -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

View File

@@ -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_;

View File

@@ -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_

View File

@@ -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_

View File

@@ -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_

View File

@@ -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_

View File

@@ -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_;
};

View File

@@ -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>(

View File

@@ -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;

View File

@@ -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

View File

@@ -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_

View File

@@ -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_

View File

@@ -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,

View File

@@ -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_;
};

View File

@@ -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_;

View File

@@ -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

View File

@@ -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

View File

@@ -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
);
}

View File

@@ -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

View File

@@ -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_;
};

View File

@@ -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

View File

@@ -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;

View File

@@ -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

View File

@@ -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:

View File

@@ -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

View File

@@ -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

View File

@@ -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_

View File

@@ -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_

View File

@@ -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)
);

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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;

View 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_

View File

@@ -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

View File

@@ -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_

View File

@@ -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");
}

View File

@@ -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

View File

@@ -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);
}
}

View File

@@ -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

View File

@@ -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
)
{

View File

@@ -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;

View File

@@ -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_

View File

@@ -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:

View File

@@ -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());
}
}

View File

@@ -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_

View File

@@ -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>

View File

@@ -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

View File

@@ -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()

View File

@@ -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());

View File

@@ -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

View File

@@ -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

View File

@@ -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

View 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

View File

@@ -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;
}

View File

@@ -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());
}

View File

@@ -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);
}

View File

@@ -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_;
}

View File

@@ -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());
}

View File

@@ -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",

View File

@@ -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

View File

@@ -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)
{

View File

@@ -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

View File

@@ -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

View File

@@ -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

View File

@@ -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_);

View File

@@ -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