From 7af70b8536b1d70d8dcb7269999e815b28176f35 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 1 May 2023 13:56:29 -0700 Subject: [PATCH] Fix delivered message kind (#2175) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: methylDragon Fix a format-security warning when building with clang (#2171) In particular, you should never have a "bare" string in a printf-like call; that could potentially access uninitialized memory. Instead, make sure to format the string with %s. Signed-off-by: Chris Lalancette Add missing stdexcept include (#2186) Signed-off-by: Øystein Sture Fix race condition in events-executor (#2177) The initial implementation of the events-executor contained a bug where the executor would end up in an inconsistent state and stop processing interrupt/shutdown notifications. Manually adding a node to the executor results in a) producing a notify waitable event and b) refreshing the executor collections. The inconsistent state would happen if the event was processed before the collections were finished to be refreshed: the executor would pick up the event but be unable to process it. This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional notify waitable events to be pushed. The behavior is observable only under heavy load. Signed-off-by: Alberto Soragna Changelog Signed-off-by: Yadunund 21.1.1 Declare rclcpp callbacks before the rcl entities (#2024) This is to ensure callbacks are destroyed last on entities destruction, avoiding the gap in time in which rmw entities hold a reference to a destroyed function. Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino add mutex to protect events_executor current entity collection (#2187) * add mutex to protect events_executor current entity collection and unit-test Signed-off-by: Alberto Soragna * be more precise with mutex locks; make stress test less stressfull Signed-off-by: Alberto Soragna * fix uncrustify error Signed-off-by: Alberto Soragna --------- Signed-off-by: Alberto Soragna Feature/available capacity of ipm (#2173) * added available_capacity to get the lowest number of free capacity for intra-process communication for a publisher Signed-off-by: Joshua Hampp * added unit tests for available_capacity Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * fixed typos in comments Signed-off-by: Joshua Hampp * Updated warning Co-authored-by: Alberto Soragna Signed-off-by: Joshua Hampp * returning 0 if ipm is disabled in lowest_available_ipm_capacity Signed-off-by: Joshua Hampp * return 0 if no subscribers are present in lowest_available_capacity Signed-off-by: Joshua Hampp * updated unit test Signed-off-by: Joshua Hampp * update unit test Signed-off-by: Joshua Hampp * moved available_capacity to a lambda function to be able to handle subscriptions which went out of scope Signed-off-by: Joshua Hampp * updated unit test to check subscriptions which went out of scope Signed-off-by: Joshua Hampp --------- Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp Co-authored-by: Joshua Hampp Co-authored-by: Joshua Hampp Co-authored-by: Alberto Soragna remove nolint since ament_cpplint updated for the c++17 header (#2198) Signed-off-by: Chen Lihui Changelog. Signed-off-by: Chris Lalancette 21.2.0 Use TRACETOOLS_ prefix for tracepoint-related macros (#2162) Signed-off-by: Christophe Bedard Remove flaky stressAddRemoveNode test (#2206) Signed-off-by: Michael Carroll Fix up misspellings of "receive". (#2208) Signed-off-by: Chris Lalancette Changelog. Signed-off-by: Chris Lalancette 21.3.0 Enable callback group tests for connextdds (#2182) * Enable callback group tests for connextdds * Enable executors and event executor tests for connextdds * Enable qos events tests for connextdds * Less flaky qos_event tests Signed-off-by: Christopher Wecht Modifies timers API to select autostart state (#2005) * Modifies timers API to select autostart state * Removes unnecessary variables * Adds autostart documentation and expands some timer test Signed-off-by: Voldivh warning: comparison of integer expressions of different signedness (#2219) https://github.com/ros2/rclcpp/pull/2167#issuecomment-1597197552 Signed-off-by: Tomoya Fujita Revamp the test_subscription.cpp tests. (#2227) The original motiviation to do this was a crash during teardown when using a newer version of gtest. But while I was in here, I did a small overall cleanup, including: 1. Moving code closer to where it is actually used. 2. Getting rid of unused 'using' statements. 3. Adding in missing includes. 4. Properly tearing down and recreating the rclcpp context during test teardown (this fixed the actual bug). 5. Making class members private where possible. 6. Renaming class methods to our usual conventions. Signed-off-by: Chris Lalancette Move always_false_v to detail namespace (#2232) Since this is a common idiom, especially under this name, we should define the `always_false_v` template within a namespace to avoid conflict with other libraries and user code. This could either be `rclcpp::detail` if it's intended only for internal use or just `rclcpp` if it's intended as a public helper. In this PR, I've initially chosen the former. Signed-off-by: Nathan Wiebe Neufeldt Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (#2224) * TypeDescriptions interface with readonly param configuration * Add parameter descriptor, to make read only * example of spinning in thread for get_type_description service * Add a basic test for the new interface * Fix tests with new parameter * Add comments about builtin parameters Signed-off-by: Emerson Knapp Signed-off-by: William Woodall Switch lifecycle to use the RCLCPP macros. (#2233) This ensures that they'll go out to /rosout and the disk. Signed-off-by: Chris Lalancette Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (#2237) Signed-off-by: Emerson Knapp Changelog. Signed-off-by: Chris Lalancette 22.0.0 Stop using constref signature of benchmark DoNotOptimize. (#2238) * Stop using constref signature of benchmark DoNotOptimize. Newer versions of google benchmark (1.8.2 in my case) warn that the compiler may optimize away the DoNotOptimize calls when using the constref version. Get away from that here by explicitly *not* calling the constref version, casting where necessary. Signed-off-by: Chris Lalancette Instrument loaned message publication code path (#2240) Signed-off-by: Christophe Bedard associated clocks should be protected by mutex. (#2255) Signed-off-by: Tomoya Fujita Change associated clocks storage to unordered_set (#2257) Signed-off-by: Luca Della Vedova Adding Missing Group Exceptions (#2256) * Adding Missing Group Exceptions Signed-off-by: CursedRock17 Signed-off-by: Chris Lalancette Co-authored-by: Tomoya Fujita Add spin_all shortcut (#2246) Signed-off-by: Tony Najjar Remove an unused variable from the events executor tests. (#2270) Signed-off-by: Chris Lalancette Add a pimpl inside rclcpp::Node for future distro backports (#2228) * Add a pimpl inside rclcpp::Node for future distro backports Signed-off-by: Emerson Knapp Co-authored-by: Chris Lalancette Adding Custom Unknown Type Error (#2272) Signed-off-by: CursedRock17 Co-authored-by: Christophe Bedard Do not crash Executor when send_response fails due to client failure. (#2276) * Do not crash Executor when send_response fails due to client failure. Related to https://github.com/ros2/ros2/issues/1253 It is not sane that a faulty client can crash our service Executor, as discussed in the referred issue, if the client is not setup properly, send_response may return RCL_RET_TIMEOUT, we should not throw an error in this case. Signed-off-by: Zang MingJie * Update rclcpp/include/rclcpp/service.hpp Co-authored-by: Tomoya Fujita Signed-off-by: Zang MingJie * address review comments. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Zang MingJie Signed-off-by: Tomoya Fujita Co-authored-by: Zang MingJie Changelog. Signed-off-by: Chris Lalancette 22.1.0 doc fix: call `canceled` only after goal state is in canceling. (#2266) Signed-off-by: Tomoya Fujita Fix a typo in a comment. (#2283) Signed-off-by: Chris Lalancette Revamp list_parameters to be more efficient and easier to read. (#2282) 1. Use constref for the loop variable. 2. Do more work outside of the loop. 3. Skip doing unnecessary work where we can inside the loop. With this in place, I measured about a 7% performance improvement over the previous implementation. Signed-off-by: Chris Lalancette Add rcl_logging_interface as an explicit dependency. (#2284) It is depended on by rclcpp/src/rclcpp/logger.cpp, but the dependency was not explicitly declared (it was being inherited from rcl, I believe). Fix that here. Signed-off-by: Chris Lalancette add logger level service to lifecycle node. (#2277) Signed-off-by: Tomoya Fujita Remove unnecessary lambda captures in the tests. (#2289) * Remove unnecessary lambda captures in the tests. This was pointed out by compiling with clang. Signed-off-by: Chris Lalancette Correct the position of a comment. (#2290) Signed-off-by: Jiaqi Li Make Rate to select the clock to work with (#2123) * Make Rate to select the clock to work with Add ROSRate respective with ROS time * Make GenericRate class to be deprecated * Adjust test cases for new rates is_steady() to be deprecated Signed-off-by: Alexey Merzlyakov Co-authored-by: Chris Lalancette Fix C++20 allocator construct deprecation (#2292) Signed-off-by: Guilherme Rodrigues Topic correct typeadapter deduction (#2294) * fix TypeAdapter deduction Signed-off-by: Chen Lihui Changelog. Signed-off-by: Chris Lalancette 22.2.0 Cleanup flaky timers_manager tests. (#2299) * Cleanup flaky timers_manager tests. The timers_manager tests were relying too heavily on specific timings; this caused them to be flaky on the buildfarm, particularly on Windows. Here, we increase the timeouts, and remove one test which just relies too heavily on specific timeouts. This should make this test much less flaky on the buildfarm. Signed-off-by: Chris Lalancette fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks (#2267) * fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks This prevents deadlocks in cases, were e.g. get_status() would be called on the handle in a callback of the handle. * test(rclcpp_action): Added test for deadlocks during access of a goal handle This test checks, if the code deadlocks, if methods on the goal handle are called from the callbacks. Signed-off-by: Janosch Machowinski Co-authored-by: Tomoya Fujita Update API docs links in package READMEs (#2302) Signed-off-by: Christophe Bedard Fix the return type of Rate::period. (#2301) In a recent commit (bc435776a257fcf76c5b0124bec26f6824342e34), we reworked how the Rate class worked so it could be used with ROS time as well. Unfortunately, we also accidentally broke the API of it by changing the return type of Rate::period to a Duration instead of a std::chrono::nanoseconds . Put this back to a std::chrono::nanoseconds; if we want to change it to a Duration, we'll have to add a new method and deprecate this one. Signed-off-by: Chris Lalancette Changelog. Signed-off-by: Chris Lalancette 23.0.0 fix the depth to relative in list_parameters (#2300) * fix the depth to relative in list_parameters Signed-off-by: leeminju531 Decouple rosout publisher init from node init. (#2174) Signed-off-by: Tomoya Fujita Documentation for list_parameters (#2315) * list_parameters documentation Signed-off-by: CursedRock17 Removing Old Connext Tests (#2313) * Removing Old Connext Tests Signed-off-by: CursedRock17 Update SignalHandler get_global_signal_handler to avoid complex types in static memory (#2316) * Update SignalHandler get_global_signal_handler to avoid complex types in static memory This was flagged by msan as a problem. There's a description of why this is a potential problem here: https://google.github.io/styleguide/cppguide.html#Static_and_Global_Variables Signed-off-by: Tully Foote Co-authored-by: William Woodall Add locking to protect the TimeSource::NodeState::node_base_ (#2320) We need this because it is possible for one thread to be handling the on_parameter_event callback while another one is detaching the node. This lock will protect that from happening. Signed-off-by: Chris Lalancette Add missing header required by the rclcpp::NodeOptions type (#2324) Signed-off-by: Ignacio Vizzo Changelog. Signed-off-by: Chris Lalancette 23.1.0 Adding API to copy all parameters from one node to another (#2304) Signed-off-by: stevemacenski remove invalid sized allocation test for SerializedMessage. (#2330) Signed-off-by: Tomoya Fujita add clients & services count (#2072) * add clients & services count * add count clients,services tests Signed-off-by: leeminju531 Changelog. Signed-off-by: Chris Lalancette 23.2.0 Fix rclcpp_lifecycle inclusion on Windows. (#2331) The comment in the commit explains this clearly, but on Windows ERROR is a macro. The reuse of it, even as an enum, causes compilation errors on downstream users. Push the macro and undefine it so downstream consumers can freely include it. Signed-off-by: Chris Lalancette Remove useless ROSRate class (#2326) Signed-off-by: Alexey Merzlyakov Fixes pointed out by the clang analyzer. (#2339) 1. Remove the default Logger copy constructor without copy assignment (rule of three -> rule of zero). 2. Remove an unnecessary capture in a lambda. 3. Mark a variable unused. 4. Mark a method as override. Signed-off-by: Chris Lalancette address rate related flaky tests. (#2329) Signed-off-by: Tomoya Fujita Adjust rclcpp usage of type description service (#2344) Signed-off-by: Michael Carroll Add missing 'enable_rosout' comments (#2345) Signed-off-by: Jiaqi Li Use message_info in SubscriptionTopicStatistics instead of typed message (#2337) * Use message_info in SubscriptionTopicStatistics instead of typed message - Untemplatize the rclcpp::topic_statistics::SubscriptionTopicStatistics class. Now we will be using message_info instead of typed deserialized messages in the handle_message callbacks. * Fix test_receive_stats_include_window_reset by using publisher emulator - Emulate publishing messages by directly calling rclcpp::Subscription::handle_message(msg_shared_ptr, message_info) and settling up needed message_info.source_timestamp Signed-off-by: Michael Orlov Co-authored-by: Tomoya Fujita Disable the loaned messages inside the executor. (#2335) * Disable the loaned messages inside the executor. They are currently unsafe to use; see the comment in the commit for more information. Signed-off-by: Chris Lalancette Add a custom deleter when constructing rcl_service_t (#2351) * Add a custom deleter when constructing rcl_service_t In the type description service construction, we were previously passing the shared_ptr to the rcl_service_t with the assumption that rclcpp::Service would do the clean up. This was an incorrect assumption, and so I have added a custom deleter to fini the service and delete when the shared_ptr is cleaned up. Signed-off-by: Michael Carroll Serialized Messages with Topic Statistics (#2274) Signed-off-by: CursedRock17 rclcpp::Time::max() clock type support. (#2352) Signed-off-by: Tomoya Fujita Updates to not use std::move in some places. (#2353) gcc 13.1.1 complains that these uses inhibit copy elision. Signed-off-by: Chris Lalancette fix (signal_handler.hpp): spelling (#2356) Signed-off-by: Zard-C Changelog. Signed-off-by: Chris Lalancette 24.0.0 fix(rclcpp_components): increase the service queue sizes in component_container (#2363) * fix(rclcpp_components): increase the service queue sizes in component_container Signed-off-by: M. Fatih Cırıt Support users holding onto shared pointers in the message memory pool (#2336) * Support users holding onto shared pointers in the message memory pool Before this commit, the MessageMemoryPool would actually reuse messages in the pool, even if the user had taken additional shared_ptr copies. This commit fixes things so that we properly handle that situation. In particular, we allocate memory during class initialization, and delete it during destruction. We then run the constructor when we hand the pointer out, and the destructor (only) when we return it to the pool. This keeps things consistent. We also add in locks, since in a multi-threaded scenario we need to protect against multiple threads accessing the pool at the same time. With this in place, things work as expected when users hold shared_ptr copies. We also add in a test for this situation. One note about performance: this update preserves the "no-allocations-at-runtime" aspect of the MessagePool. However, there are some tradeoffs with CPU time here, particularly with very large message pools. This could probably be optimized further to do less work when trying to add items back to the free_list, but I view that as a further enhancement. Signed-off-by: Chris Lalancette Fix data race in EventHandlerBase (#2349) Both the EventHandler and its associated pubs/subs share the same underlying rmw event listener. When a pub/sub is destroyed, the listener is destroyed. There is a data race when the ~EventHandlerBase wants to access the listener after it has been destroyed. The EventHandler stores a shared_ptr of its associated pub/sub. But since we were clearing the listener event callbacks on the base class destructor ~EventHandlerBase, the pub/sub was already destroyed, which means the rmw event listener was also destroyed, thus causing a segfault when trying to obtain it to clear the callbacks. Clearing the callbacks on ~EventHandler instead of ~EventHandlerBase avoids the race, since the pub/sub are still valid. Signed-off-by: Mauro Passerino aligh with rcl that a rosout publisher of a node might not exist (#2357) * aligh with rcl * keep same behavior with rclpy 1. to not throw exception durning rcl_logging_rosout_remove_sublogger 2. reset error message for RCL_RET_NOT_FOUND * test for a node with rosout disabled Signed-off-by: Chen Lihui feat(rclcpp_components): support events executor in node main template (#2366) Signed-off-by: wep21 Switch to target_link_libraries. (#2374) That way we can hide more of the implementation by using the PRIVATE keyword. Signed-off-by: Chris Lalancette Adding QoS to subscription options (#2323) * Adding QoS to subscription options Signed-off-by: CursedRock17 make type support helper supported for service (#2209) * make type support helper supported for service and action as well Signed-off-by: Chen Lihui * not to use template and only add the necessary service type currently Signed-off-by: Chen Lihui * update comment Signed-off-by: Chen Lihui * add deprecated cycle for `get_typesupport_handle` Signed-off-by: Chen Lihui --------- Signed-off-by: Chen Lihui Updated GenericSubscription to AnySubscriptionCallback (#1928) * added rclcpp::SerializedMessage support for AnySubscriptionCallback Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * using AnySubscription callback for generic subscriptiion Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * updated tests Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * Remove comment Signed-off-by: Joshua Hampp --------- Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp Co-authored-by: Joshua Hampp Co-authored-by: Jacob Perron Changelog. Signed-off-by: Chris Lalancette 25.0.0 Increase timeout for rclcpp_lifecycle to 360 (#2395) * Increase timeout for rclcpp_lifecycle to 360 Signed-off-by: Jorge Perez Co-authored-by: Chris Lalancette Stop storing the context in the guard condition. (#2400) * Stop storing the context in the guard condition. This was creating a circular reference between GuardCondition and Context, so that Context would never be cleaned up. Since we never really need the GuardCondition to know about its own Context, remove that part of the circular reference. While we are in here, we also change the get_context() lambda to a straight weak_ptr; there is no reason for the indirection since the context for the guard condition cannot change at runtime. We also remove the deprecated version of the get_notify_guard_condition(). That's because there is no way to properly implement it in the new scheme, and it seems to be unused outside of rclcpp. Finally, we add in a test that guarantees the use_count is what we expect when inside and leaving a scope, ensuring that contexts will properly be destroyed. Signed-off-by: Chris Lalancette Add transient local durability support to publisher and subscriptions when using intra-process communication (#2303) * Add intra process transient local durability support to publisher and subscription Signed-off-by: Jeffery Hsu * Remove durability_is_transient_local_ from publisher_base Signed-off-by: Jeffery Hsu * Design changes that move most transient local publish functionalities out of intra process manager into intra process manager Signed-off-by: Jeffery Hsu * Move transient local publish to a separate function Signed-off-by: Jeffery Hsu * Remove publisher buffer weak ptr from intra process manager when it associated publisher is removed. Signed-off-by: Jeffery Hsu * Remove incorrectly placed RCLCPP_PUBLIC Signed-off-by: Jeffery Hsu * Add missing RCLCPP_PUBLIC Signed-off-by: Jeffery Hsu * Expand RingBufferImplementation beyond shared_ptr and unique_ptr Signed-off-by: Jeffery Hsu * Comment and format fix Signed-off-by: Jeffery Hsu --------- Signed-off-by: Jeffery Hsu Increase the cppcheck timeout to 600 seconds. (#2409) Signed-off-by: Chris Lalancette Changelog. Signed-off-by: Chris Lalancette 26.0.0 Make sure to mark RingBuffer methods as 'override'. (#2410) This gets rid of a warning when building under clang. Signed-off-by: Chris Lalancette Removed deprecated header (#2413) Signed-off-by: Alejandro Hernández Cordero [events executor] - Fix Behavior with Timer Cancel (#2375) * fix Signed-off-by: Matt Condino * add timer cancel tests Signed-off-by: Matt Condino * cleanup header include Signed-off-by: Matt Condino * reverting change to timer_greater function Signed-off-by: Gus Brigantino * use std::optional, and handle edgecase of 1 cancelled timer Signed-off-by: Matt Condino * clean up run_timers func Signed-off-by: Gus Brigantino * some fixes and added tests for cancel then reset of timers. Signed-off-by: Matt Condino * refactor and clean up. remove cancelled timer tracking. Signed-off-by: Matt Condino * remove unused method for size() Signed-off-by: Matt Condino * linting Signed-off-by: Matt Condino * relax timing constraints in tests Signed-off-by: Matt Condino * further relax timing constraints to ensure windows tests are not flaky. Signed-off-by: Matt Condino * use sim clock for tests, pub clock at .25 realtime rate. Signed-off-by: Matt Condino --------- Signed-off-by: Matt Condino Signed-off-by: Gus Brigantino Co-authored-by: Gus Brigantino Split test_executors up into smaller chunks. (#2421) The original reason is that on Windows Debug, we were failing to compile because the compilation unit was too large. But also this file was way too large (1200 lines), so it makes sense to split it up. Signed-off-by: Chris Lalancette Changelog. Signed-off-by: Chris Lalancette 27.0.0 feat: add/minus for msg::Time and rclcpp::Duration (#2419) * feat: add/minus for msg::Time and rclcpp::Duration Signed-off-by: HuaTsai crash on no class found (#2415) * crash on no class found * error on no class found instead of no callback groups Signed-off-by: Adam Aposhian Co-authored-by: Chris Lalancette Update quality declaration documents (#2427) Signed-off-by: Christophe Bedard Set hints to find the python version we actually want. (#2426) The comment in the commit explains the reasoning behind it. Signed-off-by: Chris Lalancette fix doxygen syntax for NodeInterfaces (#2428) Signed-off-by: Jonas Otto Remove the set_deprecated signatures in any_subscription_callback. (#2431) These have been deprecated since April 2021, so it is safe to remove them now. Signed-off-by: Chris Lalancette Add EXECUTOR docs (#2440) Signed-off-by: Ruddick Lawrence <679360+mrjogo@users.noreply.github.com> Various cleanups to deal with uncrustify 0.78. (#2439) These should also work with uncrustify 0.72. Signed-off-by: Chris Lalancette Rule of five: implement move operators (#2425) Signed-off-by: Tim Clephas Implement generic client (#2358) * Implement generic client Signed-off-by: Barry Xu * Fix the incorrect parameter declaration Signed-off-by: Barry Xu * Deleted copy constructor and assignment for FutureAndRequestId Signed-off-by: Barry Xu * Update codes after rebase Signed-off-by: Barry Xu * Address review comments Signed-off-by: Barry Xu * Address review comments from iuhilnehc-ynos Signed-off-by: Barry Xu * Correct an error in a description Signed-off-by: Barry Xu * Fix window build errors Signed-off-by: Barry Xu * Address review comments from William Signed-off-by: Barry Xu * Add doc strings to create_generic_client Signed-off-by: Barry Xu --------- Signed-off-by: Barry Xu Fix TypeAdapted publishing with large messages. (#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. Signed-off-by: Chris Lalancette Modify rclcpp_action::GoalUUID hashing algorithm (#2441) * Add unit tests for hashing rclcpp_action::GoalUUID's * Use the FNV-1a hash algorithm for Goal UUID Signed-off-by: Mauro Passerino relax the test simulation rate for timer canceling tests. (#2453) Signed-off-by: Tomoya Fujita Revert "relax the test simulation rate for timer canceling tests. (#2453)" (#2456) This reverts commit 1c350d0d7fb9c7158e0a39057112486ddbd38e9a. Signed-off-by: Tomoya Fujita enable simulation clock for timer canceling test. (#2458) * enable simulation clock for timer canceling test. Signed-off-by: Tomoya Fujita * move MainExecutorTypes to test_executors_timer_cancel_behavior.cpp. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita refactor and improve the parameterized spin_some tests for executors (#2460) * refactor and improve the spin_some parameterized tests for executors Signed-off-by: William Woodall * disable spin_some_max_duration for the StaticSingleThreadedExecutor and EventsExecutor Signed-off-by: William Woodall * fixup and clarify the docstring for Executor::spin_some() Signed-off-by: William Woodall * style Signed-off-by: William Woodall * review comments Signed-off-by: William Woodall --------- Signed-off-by: William Woodall fix spin_some_max_duration unit-test for events-executor (#2465) Signed-off-by: Alberto Soragna Do not generate the exception when action service response timeout. (#2464) * Do not generate the exception when action service response timeout. Signed-off-by: Tomoya Fujita * address review comment. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita Changelog. Signed-off-by: Marco A. Gutierrez 28.0.0 Signed-off-by: Marco A. Gutierrez fix flakiness in TestTimersManager unit-test (#2468) the previous version of the test was relying on the assumption that a timer with 1ms period gets called at least 6 times if the main thread waits 15ms. this is true most of the times, but it's not guaranteed, especially when running the test on windows CI servers. the new version of the test makes no assumptions on how much time it takes for the timers manager to invoke the timers, but rather focuses on ensuring that they are called the right amount of times, which is what's important for the purpose of the test Signed-off-by: Alberto Soragna Utilize rclcpp::WaitSet as part of the executors (#2142) * Deprecate callback_group call taking context Signed-off-by: Michael Carroll * Add base executor objects that can be used by implementors Signed-off-by: Michael Carroll * Template common operations Signed-off-by: Michael Carroll * Address reviewer feedback: * Add callback to EntitiesCollector constructor * Make function to check automatically added callback groups take a list Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Address reviewer feedback and fix templates Signed-off-by: Michael Carroll * Lint and docs Signed-off-by: Michael Carroll * Make executor own the notify waitable Signed-off-by: Michael Carroll * Add pending queue to collector, remove from waitable Also change node's get_guard_condition to return shared_ptr Signed-off-by: Michael Carroll * Change interrupt guard condition to shared_ptr Check if guard condition is valid before adding it to the waitable Signed-off-by: Michael Carroll * Lint and docs Signed-off-by: Michael Carroll * Utilize rclcpp::WaitSet as part of the executors Signed-off-by: Michael Carroll * Don't exchange atomic twice Signed-off-by: Michael Carroll * Fix add_node and add more tests Signed-off-by: Michael Carroll * Make get_notify_guard_condition follow API tick-tock Signed-off-by: Michael Carroll * Improve callback group tick-tocking Signed-off-by: Michael Carroll * Don't lock twice Signed-off-by: Michael Carroll * Address reviewer feedback Signed-off-by: Michael Carroll * Add thread safety annotations and make locks consistent Signed-off-by: Michael Carroll * @wip Signed-off-by: Michael Carroll * Reset callback groups for multithreaded executor Signed-off-by: Michael Carroll * Avoid many small function calls when building executables Signed-off-by: Michael Carroll * Re-trigger guard condition if buffer has data Signed-off-by: Michael Carroll * Address reviewer feedback Signed-off-by: Michael Carroll * Trace points Signed-off-by: Michael Carroll * Remove tracepoints Signed-off-by: Michael Carroll * Reducing diff Signed-off-by: Michael Carroll * Reduce diff Signed-off-by: Michael Carroll * Uncrustify Signed-off-by: Michael Carroll * Restore tests Signed-off-by: Michael Carroll * Back to weak_ptr and reduce test time Signed-off-by: Michael Carroll * reduce diff and lint Signed-off-by: Michael Carroll * Restore static single threaded tests that weren't working before Signed-off-by: Michael Carroll * Restore more tests Signed-off-by: Michael Carroll * Fix multithreaded test Signed-off-by: Michael Carroll * Fix assert Signed-off-by: Michael Carroll * Fix constructor test Signed-off-by: Michael Carroll * Change ready_executables signature back Signed-off-by: Michael Carroll * Don't enforce removing callback groups before nodes Signed-off-by: Michael Carroll * Remove the "add_valid_node" API Signed-off-by: Michael Carroll * Only notify if the trigger condition is valid Signed-off-by: Michael Carroll * Only trigger if valid and needed Signed-off-by: Michael Carroll * Fix spin_some/spin_all implementation Signed-off-by: Michael Carroll * Restore single threaded executor Signed-off-by: Michael Carroll * Picking ABI-incompatible executor changes Signed-off-by: Michael Carroll * Add PIMPL Signed-off-by: Michael Carroll * Additional waitset prune Signed-off-by: Michael Carroll * Fix bad merge Signed-off-by: Michael Carroll * Expand test timeout Signed-off-by: Michael Carroll * Introduce method to clear expired entities from a collection Signed-off-by: Michael Carroll * Make sure to call remove_expired_entities(). Signed-off-by: Chris Lalancette * Prune queued work when callback group is removed Signed-off-by: Michael Carroll * Prune subscriptions from dynamic storage Signed-off-by: Michael Carroll * Styles fixes. Signed-off-by: Chris Lalancette * Re-trigger guard conditions Signed-off-by: Michael Carroll * Condense to just use watiable.take_data Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Address reviewer comments (nits) Signed-off-by: Michael Carroll * Lock mutex when copying Signed-off-by: Michael Carroll * Refactors to static single threaded based on reviewers Signed-off-by: Michael Carroll * More small refactoring Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Add ready executable accessors to WaitResult Signed-off-by: Michael Carroll * Make use of accessors from wait_set Signed-off-by: Michael Carroll * Fix tests Signed-off-by: Michael Carroll * Fix more tests Signed-off-by: Michael Carroll * Tidy up single threaded executor implementation Signed-off-by: Michael Carroll * Don't null out timer, rely on call Signed-off-by: Michael Carroll * change how timers are checked from wait result in executors Signed-off-by: William Woodall * peak -> peek Signed-off-by: William Woodall * fix bug in next_waitable logic Signed-off-by: William Woodall * fix bug in StaticSTE that broke the add callback groups to executor tests Signed-off-by: William Woodall * style Signed-off-by: William Woodall --------- Signed-off-by: Michael Carroll Signed-off-by: Michael Carroll Signed-off-by: Chris Lalancette Signed-off-by: William Woodall Co-authored-by: Chris Lalancette Co-authored-by: William Woodall update rclcpp::Waitable API to use references and const (#2467) Signed-off-by: William Woodall Add tracepoint for generic publisher/subscriber (#2448) Signed-off-by: h-suzuki --- rclcpp/CHANGELOG.rst | 172 ++++ rclcpp/CMakeLists.txt | 97 ++- rclcpp/QUALITY_DECLARATION.md | 24 +- rclcpp/README.md | 2 +- rclcpp/include/rclcpp/any_executable.hpp | 6 +- .../include/rclcpp/any_service_callback.hpp | 8 +- .../rclcpp/any_subscription_callback.hpp | 119 ++- rclcpp/include/rclcpp/callback_group.hpp | 44 +- rclcpp/include/rclcpp/client.hpp | 55 +- rclcpp/include/rclcpp/context.hpp | 1 + .../rclcpp/copy_all_parameter_values.hpp | 82 ++ .../include/rclcpp/create_generic_client.hpp | 90 ++ .../rclcpp/create_generic_subscription.hpp | 15 +- rclcpp/include/rclcpp/create_subscription.hpp | 19 +- rclcpp/include/rclcpp/create_timer.hpp | 23 +- .../resolve_intra_process_buffer_type.hpp | 5 + rclcpp/include/rclcpp/duration.hpp | 9 + rclcpp/include/rclcpp/event_handler.hpp | 16 +- .../include/rclcpp/exceptions/exceptions.hpp | 16 + rclcpp/include/rclcpp/executor.hpp | 216 ++--- rclcpp/include/rclcpp/executors.hpp | 12 + .../executor_entities_collection.hpp | 6 + .../executors/executor_notify_waitable.hpp | 10 +- .../static_executor_entities_collector.hpp | 357 -------- .../static_single_threaded_executor.hpp | 111 +-- .../buffers/buffer_implementation_base.hpp | 5 + .../buffers/intra_process_buffer.hpp | 87 +- .../buffers/ring_buffer_implementation.hpp | 119 ++- .../events_executor/events_executor.hpp | 10 +- .../experimental/intra_process_manager.hpp | 142 ++- .../subscription_intra_process.hpp | 9 +- .../subscription_intra_process_base.hpp | 15 +- .../subscription_intra_process_buffer.hpp | 19 +- .../rclcpp/experimental/timers_manager.hpp | 14 +- rclcpp/include/rclcpp/generic_client.hpp | 207 +++++ rclcpp/include/rclcpp/generic_publisher.hpp | 2 +- .../include/rclcpp/generic_subscription.hpp | 25 +- rclcpp/include/rclcpp/guard_condition.hpp | 14 +- rclcpp/include/rclcpp/logger.hpp | 3 - rclcpp/include/rclcpp/node.hpp | 71 +- rclcpp/include/rclcpp/node_impl.hpp | 12 +- .../detail/node_interfaces_helpers.hpp | 4 +- .../rclcpp/node_interfaces/node_graph.hpp | 8 + .../node_interfaces/node_graph_interface.hpp | 18 + .../node_interfaces/node_interfaces.hpp | 6 +- .../node_type_descriptions.hpp | 63 ++ .../node_type_descriptions_interface.hpp | 44 + rclcpp/include/rclcpp/node_options.hpp | 1 + .../rclcpp/parameter_events_filter.hpp | 2 +- rclcpp/include/rclcpp/parameter_value.hpp | 1 + rclcpp/include/rclcpp/publisher.hpp | 82 +- rclcpp/include/rclcpp/publisher_base.hpp | 17 + rclcpp/include/rclcpp/publisher_options.hpp | 4 + rclcpp/include/rclcpp/qos_event.hpp | 22 - rclcpp/include/rclcpp/rate.hpp | 79 +- rclcpp/include/rclcpp/rclcpp.hpp | 5 + rclcpp/include/rclcpp/service.hpp | 24 +- .../strategies/allocator_memory_strategy.hpp | 4 +- .../message_pool_memory_strategy.hpp | 110 ++- rclcpp/include/rclcpp/subscription.hpp | 35 +- rclcpp/include/rclcpp/subscription_base.hpp | 17 +- .../include/rclcpp/subscription_factory.hpp | 5 +- .../include/rclcpp/subscription_options.hpp | 4 + rclcpp/include/rclcpp/time.hpp | 23 +- rclcpp/include/rclcpp/timer.hpp | 28 +- .../subscription_topic_statistics.hpp | 23 +- rclcpp/include/rclcpp/typesupport_helpers.hpp | 39 + rclcpp/include/rclcpp/wait_result.hpp | 170 +++- rclcpp/include/rclcpp/wait_result_kind.hpp | 1 + .../detail/storage_policy_common.hpp | 78 +- .../wait_set_policies/dynamic_storage.hpp | 75 +- .../sequential_synchronization.hpp | 2 +- .../wait_set_policies/static_storage.hpp | 55 ++ rclcpp/include/rclcpp/wait_set_template.hpp | 19 + rclcpp/include/rclcpp/waitable.hpp | 57 +- rclcpp/package.xml | 5 +- rclcpp/src/rclcpp/any_executable.cpp | 1 + rclcpp/src/rclcpp/callback_group.cpp | 35 +- rclcpp/src/rclcpp/client.cpp | 2 +- rclcpp/src/rclcpp/context.cpp | 4 +- rclcpp/src/rclcpp/create_generic_client.cpp | 44 + .../resolve_intra_process_buffer_type.cpp | 37 + rclcpp/src/rclcpp/duration.cpp | 48 ++ rclcpp/src/rclcpp/event_handler.cpp | 15 +- rclcpp/src/rclcpp/executor.cpp | 811 +++++++----------- rclcpp/src/rclcpp/executors.cpp | 15 + .../executor_entities_collection.cpp | 31 +- .../executors/executor_entities_collector.cpp | 28 +- .../executors/executor_notify_waitable.cpp | 37 +- .../executors/multi_threaded_executor.cpp | 14 +- .../executors/single_threaded_executor.cpp | 5 + .../static_executor_entities_collector.cpp | 524 ----------- .../static_single_threaded_executor.cpp | 267 ++---- .../events_executor/events_executor.cpp | 93 +- .../rclcpp/experimental/timers_manager.cpp | 49 +- rclcpp/src/rclcpp/generic_client.cpp | 164 ++++ rclcpp/src/rclcpp/generic_publisher.cpp | 5 + rclcpp/src/rclcpp/generic_subscription.cpp | 4 +- rclcpp/src/rclcpp/guard_condition.cpp | 23 +- rclcpp/src/rclcpp/intra_process_manager.cpp | 85 +- rclcpp/src/rclcpp/logger.cpp | 11 +- rclcpp/src/rclcpp/logging_mutex.cpp | 1 + rclcpp/src/rclcpp/node.cpp | 62 +- .../src/rclcpp/node_interfaces/node_base.cpp | 53 +- .../src/rclcpp/node_interfaces/node_graph.cpp | 44 + .../node_interfaces/node_parameters.cpp | 69 +- .../rclcpp/node_interfaces/node_services.cpp | 6 +- .../rclcpp/node_interfaces/node_timers.cpp | 5 +- .../rclcpp/node_interfaces/node_topics.cpp | 5 +- .../node_type_descriptions.cpp | 153 ++++ .../rclcpp/node_interfaces/node_waitables.cpp | 3 +- rclcpp/src/rclcpp/parameter_value.cpp | 3 +- rclcpp/src/rclcpp/publisher_base.cpp | 26 + rclcpp/src/rclcpp/rate.cpp | 106 +++ rclcpp/src/rclcpp/signal_handler.cpp | 2 +- rclcpp/src/rclcpp/signal_handler.hpp | 4 +- rclcpp/src/rclcpp/subscription_base.cpp | 28 +- .../subscription_intra_process_base.cpp | 10 +- rclcpp/src/rclcpp/time.cpp | 40 +- rclcpp/src/rclcpp/time_source.cpp | 41 +- rclcpp/src/rclcpp/timer.cpp | 9 +- rclcpp/src/rclcpp/typesupport_helpers.cpp | 80 +- rclcpp/src/rclcpp/waitable.cpp | 79 ++ rclcpp/test/benchmark/CMakeLists.txt | 9 +- rclcpp/test/benchmark/benchmark_executor.cpp | 39 - rclcpp/test/msg/LargeMessage.msg | 3 + rclcpp/test/rclcpp/CMakeLists.txt | 496 +++-------- .../test/rclcpp/executors/executor_types.hpp | 70 ++ .../rclcpp/executors/test_events_executor.cpp | 67 +- .../test/rclcpp/executors/test_executors.cpp | 617 +++++++------ .../executors/test_executors_intraprocess.cpp | 125 +++ .../test_executors_timer_cancel_behavior.cpp | 430 ++++++++++ ...est_static_executor_entities_collector.cpp | 14 +- .../test_static_single_threaded_executor.cpp | 11 +- .../node_interfaces/test_node_graph.cpp | 19 + .../node_interfaces/test_node_parameters.cpp | 17 +- .../node_interfaces/test_node_services.cpp | 4 +- .../node_interfaces/test_node_timers.cpp | 2 +- .../test_node_type_descriptions.cpp | 61 ++ .../node_interfaces/test_node_waitables.cpp | 11 +- .../test_allocator_memory_strategy.cpp | 29 +- .../test_message_pool_memory_strategy.cpp | 30 +- .../test_add_callback_groups_to_executor.cpp | 85 +- .../test/rclcpp/test_any_service_callback.cpp | 29 +- .../rclcpp/test_any_subscription_callback.cpp | 2 - rclcpp/test/rclcpp/test_client.cpp | 385 --------- rclcpp/test/rclcpp/test_client_common.cpp | 591 +++++++++++++ rclcpp/test/rclcpp/test_context.cpp | 22 + .../rclcpp/test_copy_all_parameter_values.cpp | 88 ++ rclcpp/test/rclcpp/test_create_timer.cpp | 26 + rclcpp/test/rclcpp/test_duration.cpp | 29 + rclcpp/test/rclcpp/test_executor.cpp | 175 ++-- rclcpp/test/rclcpp/test_generic_client.cpp | 230 +++++ rclcpp/test/rclcpp/test_generic_pubsub.cpp | 50 +- rclcpp/test/rclcpp/test_guard_condition.cpp | 24 +- .../test/rclcpp/test_intra_process_buffer.cpp | 126 +++ .../rclcpp/test_intra_process_manager.cpp | 504 ++++++++--- rclcpp/test/rclcpp/test_memory_strategy.cpp | 18 +- rclcpp/test/rclcpp/test_node.cpp | 4 + rclcpp/test/rclcpp/test_parameter_client.cpp | 14 +- rclcpp/test/rclcpp/test_publisher.cpp | 138 ++- .../test_publisher_with_type_adapter.cpp | 128 ++- rclcpp/test/rclcpp/test_qos_event.cpp | 80 +- rclcpp/test/rclcpp/test_rate.cpp | 271 +++++- .../test_ring_buffer_implementation.cpp | 70 +- .../test/rclcpp/test_rosout_subscription.cpp | 30 +- .../test/rclcpp/test_serialized_message.cpp | 5 - rclcpp/test/rclcpp/test_service.cpp | 78 +- rclcpp/test/rclcpp/test_subscription.cpp | 446 +++++----- .../test/rclcpp/test_subscription_options.cpp | 3 + rclcpp/test/rclcpp/test_time.cpp | 25 +- rclcpp/test/rclcpp/test_time_source.cpp | 2 +- rclcpp/test/rclcpp/test_timer.cpp | 52 +- rclcpp/test/rclcpp/test_timers_manager.cpp | 158 ++-- .../test/rclcpp/test_typesupport_helpers.cpp | 53 +- .../test_subscription_topic_statistics.cpp | 314 ++----- .../test_dynamic_storage.cpp | 6 +- .../wait_set_policies/test_static_storage.cpp | 6 +- .../test_storage_policy_common.cpp | 6 +- .../test_thread_safe_synchronization.cpp | 6 +- rclcpp_action/CHANGELOG.rst | 62 ++ rclcpp_action/CMakeLists.txt | 62 +- rclcpp_action/QUALITY_DECLARATION.md | 14 +- rclcpp_action/README.md | 3 +- .../include/rclcpp_action/client.hpp | 6 +- .../rclcpp_action/client_goal_handle.hpp | 2 +- .../rclcpp_action/client_goal_handle_impl.hpp | 22 +- .../include/rclcpp_action/server.hpp | 12 +- .../rclcpp_action/server_goal_handle.hpp | 4 +- rclcpp_action/include/rclcpp_action/types.hpp | 15 +- rclcpp_action/package.xml | 3 +- rclcpp_action/src/client.cpp | 13 +- rclcpp_action/src/server.cpp | 55 +- rclcpp_action/test/benchmark/CMakeLists.txt | 6 +- rclcpp_action/test/test_client.cpp | 102 ++- rclcpp_action/test/test_server.cpp | 6 +- rclcpp_action/test/test_types.cpp | 33 + rclcpp_components/CHANGELOG.rst | 57 ++ rclcpp_components/CMakeLists.txt | 32 +- rclcpp_components/QUALITY_DECLARATION.md | 16 +- rclcpp_components/README.md | 2 +- .../rclcpp_components_register_node.cmake | 11 +- .../rclcpp_components/node_factory.hpp | 1 + rclcpp_components/package.xml | 2 +- rclcpp_components/src/component_manager.cpp | 6 +- rclcpp_components/src/node_main.cpp.in | 61 +- rclcpp_lifecycle/CHANGELOG.rst | 60 ++ rclcpp_lifecycle/CMakeLists.txt | 5 + rclcpp_lifecycle/QUALITY_DECLARATION.md | 18 +- rclcpp_lifecycle/README.md | 3 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 26 + .../lifecycle_node_interface.hpp | 12 + rclcpp_lifecycle/package.xml | 2 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 31 +- .../src/lifecycle_node_interface_impl.cpp | 36 +- .../src/lifecycle_node_interface_impl.hpp | 6 +- .../benchmark/benchmark_lifecycle_client.cpp | 11 +- .../benchmark/benchmark_lifecycle_node.cpp | 22 +- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 75 +- 219 files changed, 8745 insertions(+), 5083 deletions(-) create mode 100644 rclcpp/include/rclcpp/copy_all_parameter_values.hpp create mode 100644 rclcpp/include/rclcpp/create_generic_client.hpp delete mode 100644 rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp create mode 100644 rclcpp/include/rclcpp/generic_client.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp delete mode 100644 rclcpp/include/rclcpp/qos_event.hpp create mode 100644 rclcpp/src/rclcpp/create_generic_client.cpp create mode 100644 rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp delete mode 100644 rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp create mode 100644 rclcpp/src/rclcpp/generic_client.cpp create mode 100644 rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp create mode 100644 rclcpp/src/rclcpp/rate.cpp create mode 100644 rclcpp/test/msg/LargeMessage.msg create mode 100644 rclcpp/test/rclcpp/executors/executor_types.hpp create mode 100644 rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp create mode 100644 rclcpp/test/rclcpp/test_client_common.cpp create mode 100644 rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp create mode 100644 rclcpp/test/rclcpp/test_generic_client.cpp diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 32830939c8..c02354634b 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,178 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.0 (2024-03-28) +------------------- +* fix spin_some_max_duration unit-test for events-executor (`#2465 `_) +* refactor and improve the parameterized spin_some tests for executors (`#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 `_) + * 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 `_)" (`#2456 `_) + This reverts commit 1c350d0d7fb9c7158e0a39057112486ddbd38e9a. +* relax the test simulation rate for timer canceling tests. (`#2453 `_) +* Fix TypeAdapted publishing with large messages. (`#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 `_) + * 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 `_) +* Various cleanups to deal with uncrustify 0.78. (`#2439 `_) + These should also work with uncrustify 0.72. +* Remove the set_deprecated signatures in any_subscription_callback. (`#2431 `_) + These have been deprecated since April 2021, so it is safe + to remove them now. +* fix doxygen syntax for NodeInterfaces (`#2428 `_) +* Set hints to find the python version we actually want. (`#2426 `_) + The comment in the commit explains the reasoning behind it. +* Update quality declaration documents (`#2427 `_) +* feat: add/minus for msg::Time and rclcpp::Duration (`#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 `_) +* [events executor] - Fix Behavior with Timer Cancel (`#2375 `_) +* Removed deprecated header (`#2413 `_) +* Make sure to mark RingBuffer methods as 'override'. (`#2410 `_) +* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Matt Condino + +26.0.0 (2024-01-24) +------------------- +* Increase the cppcheck timeout to 600 seconds. (`#2409 `_) +* Add transient local durability support to publisher and subscriptions when using intra-process communication (`#2303 `_) +* Stop storing the context in the guard condition. (`#2400 `_) +* Contributors: Chris Lalancette, Jeffery Hsu + +25.0.0 (2023-12-26) +------------------- +* Updated GenericSubscription to AnySubscriptionCallback (`#1928 `_) +* make type support helper supported for service (`#2209 `_) +* Adding QoS to subscription options (`#2323 `_) +* Switch to target_link_libraries. (`#2374 `_) +* aligh with rcl that a rosout publisher of a node might not exist (`#2357 `_) +* Fix data race in EventHandlerBase (`#2349 `_) +* Support users holding onto shared pointers in the message memory pool (`#2336 `_) +* Contributors: Chen Lihui, Chris Lalancette, DensoADAS, Lucas Wendland, mauropasse + +24.0.0 (2023-11-06) +------------------- +* fix (signal_handler.hpp): spelling (`#2356 `_) +* Updates to not use std::move in some places. (`#2353 `_) +* rclcpp::Time::max() clock type support. (`#2352 `_) +* Serialized Messages with Topic Statistics (`#2274 `_) +* Add a custom deleter when constructing rcl_service_t (`#2351 `_) +* Disable the loaned messages inside the executor. (`#2335 `_) +* Use message_info in SubscriptionTopicStatistics instead of typed message (`#2337 `_) +* Add missing 'enable_rosout' comments (`#2345 `_) +* Adjust rclcpp usage of type description service (`#2344 `_) +* address rate related flaky tests. (`#2329 `_) +* Fixes pointed out by the clang analyzer. (`#2339 `_) +* Remove useless ROSRate class (`#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 `_) +* remove invalid sized allocation test for SerializedMessage. (`#2330 `_) +* Adding API to copy all parameters from one node to another (`#2304 `_) +* Contributors: Minju, Lee, Steve Macenski, Tomoya Fujita + +23.1.0 (2023-10-04) +------------------- +* Add locking to protect the TimeSource::NodeState::node_base\_ (`#2320 `_) +* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 `_) +* Removing Old Connext Tests (`#2313 `_) +* Documentation for list_parameters (`#2315 `_) +* Decouple rosout publisher init from node init. (`#2174 `_) +* fix the depth to relative in list_parameters (`#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 `_) +* Update API docs links in package READMEs (`#2302 `_) +* Cleanup flaky timers_manager tests. (`#2299 `_) +* Contributors: Chris Lalancette, Christophe Bedard + +22.2.0 (2023-09-07) +------------------- +* Topic correct typeadapter deduction (`#2294 `_) +* Fix C++20 allocator construct deprecation (`#2292 `_) +* Make Rate to select the clock to work with (`#2123 `_) +* Correct the position of a comment. (`#2290 `_) +* Remove unnecessary lambda captures in the tests. (`#2289 `_) +* Add rcl_logging_interface as an explicit dependency. (`#2284 `_) +* Revamp list_parameters to be more efficient and easier to read. (`#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 `_) +* Adding Custom Unknown Type Error (`#2272 `_) +* Add a pimpl inside rclcpp::Node for future distro backports (`#2228 `_) +* Remove an unused variable from the events executor tests. (`#2270 `_) +* Add spin_all shortcut (`#2246 `_) +* Adding Missing Group Exceptions (`#2256 `_) +* Change associated clocks storage to unordered_set (`#2257 `_) +* associated clocks should be protected by mutex. (`#2255 `_) +* Instrument loaned message publication code path (`#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 `_) +* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) +* Move always_false_v to detail namespace (`#2232 `_) +* Revamp the test_subscription.cpp tests. (`#2227 `_) +* warning: comparison of integer expressions of different signedness (`#2219 `_) +* Modifies timers API to select autostart state (`#2005 `_) +* Enable callback group tests for connextdds (`#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 `_) +* Remove flaky stressAddRemoveNode test (`#2206 `_) +* Use TRACETOOLS\_ prefix for tracepoint-related macros (`#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 `_) +* Feature/available capacity of ipm (`#2173 `_) +* add mutex to protect events_executor current entity collection (`#2187 `_) +* Declare rclcpp callbacks before the rcl entities (`#2024 `_) +* Contributors: Alberto Soragna, Chen Lihui, DensoADAS, mauropasse + +21.1.1 (2023-05-11) +------------------- +* Fix race condition in events-executor (`#2177 `_) +* Add missing stdexcept include (`#2186 `_) +* Fix a format-security warning when building with clang (`#2171 `_) +* Fix delivered message kind (`#2175 `_) +* Contributors: Alberto Soragna, Chris Lalancette, methylDragon, Øystein Sture + 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index e22f3f240a..372038e9f3 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.12) +cmake_minimum_required(VERSION 3.20) project(rclcpp) @@ -10,11 +10,14 @@ 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) @@ -42,7 +45,9 @@ 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,12 +69,12 @@ set(${PROJECT_NAME}_SRCS 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 @@ -92,6 +97,7 @@ 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 @@ -106,6 +112,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/serialization.cpp src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp @@ -122,6 +129,21 @@ 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 @@ -200,22 +222,28 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" "$") -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" +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 ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -237,20 +265,23 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# 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) +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 +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -268,7 +299,7 @@ install( if(TEST cppcheck) # must set the property after ament_package() - set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) + set_tests_properties(cppcheck PROPERTIES TIMEOUT 600) endif() if(TEST cpplint) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index e0913167c2..53c2908f7c 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -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 Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +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). ## 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/master/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/rolling/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/master/rclcpp/test/benchmark). +The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/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 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. +`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. 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/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/rolling/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/master/rcl/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/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/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/rolling/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/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/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/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/rolling/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/master/rmw/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/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/master/statistics_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/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://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/ros2_tracing/blob/rolling/tracetools/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp/README.md b/rclcpp/README.md index 55c25f4780..d3c390b449 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -2,7 +2,7 @@ The ROS client library in C++. -Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components and features. +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/). ## Quality Declaration diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 5d4064f452..e4e9eaecb0 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -45,9 +45,9 @@ struct AnyExecutable rclcpp::ClientBase::SharedPtr client; rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; - std::shared_ptr data; + rclcpp::CallbackGroup::SharedPtr callback_group {nullptr}; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr}; + std::shared_ptr data {nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index f8c2592fc5..918d8e5a29 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -156,7 +156,7 @@ class AnyServiceCallback const std::shared_ptr & request_header, std::shared_ptr request) { - TRACEPOINT(callback_start, static_cast(this), false); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); if (std::holds_alternative(callback_)) { // TODO(ivanpauno): Remove the set method, and force the users of this class // to pass a callback at construnciton. @@ -182,7 +182,7 @@ class AnyServiceCallback const auto & cb = std::get(callback_); cb(request_header, std::move(request), response); } - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); return response; } @@ -191,9 +191,9 @@ class AnyServiceCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && arg) { - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(arg); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, static_cast(this), symbol); diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 65b29d8535..da5abe6c53 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -20,7 +20,7 @@ #include #include #include -#include // NOLINT[build/include_order] +#include #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 -inline constexpr bool always_false_v = false; - namespace rclcpp { namespace detail { +template +inline constexpr bool always_false_v = false; + template struct MessageDeleterHelper { @@ -158,13 +158,14 @@ struct AnySubscriptionCallbackPossibleTypes template< typename MessageT, typename AllocatorT, - bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value + bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value, + bool is_serialized_type = serialization_traits::is_serialized_message_class::value > struct AnySubscriptionCallbackHelper; /// Specialization for when MessageT is not a TypeAdapter. template -struct AnySubscriptionCallbackHelper +struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; @@ -194,7 +195,7 @@ struct AnySubscriptionCallbackHelper /// Specialization for when MessageT is a TypeAdapter. template -struct AnySubscriptionCallbackHelper +struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; @@ -232,6 +233,26 @@ struct AnySubscriptionCallbackHelper >; }; +/// Specialization for when MessageT is a SerializedMessage to exclude duplicated declarations. +template +struct AnySubscriptionCallbackHelper +{ + using CallbackTypes = AnySubscriptionCallbackPossibleTypes; + + 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< @@ -372,58 +393,12 @@ class AnySubscriptionCallback // converted to one another, e.g. shared_ptr and unique_ptr. using scbth = detail::SubscriptionCallbackTypeHelper; - // 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)> - >::value - || - rclcpp::function_traits::same_arguments< - typename scbth::callback_type, - std::function, 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(callback)); - } else { - // Otherwise just assign it. - callback_variant_ = static_cast(callback); - } + callback_variant_ = static_cast(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 -#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) - // suppress deprecation warnings in `test_any_subscription_callback.cpp` - [[deprecated("use 'void(std::shared_ptr)' instead")]] -#endif - void - set_deprecated(std::function)> callback) - { - callback_variant_ = callback; - } - - /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. - template -#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) - // suppress deprecation warnings in `test_any_subscription_callback.cpp` - [[deprecated( - "use 'void(std::shared_ptr, const rclcpp::MessageInfo &)' instead" - )]] -#endif - void - set_deprecated(std::function, const rclcpp::MessageInfo &)> callback) - { - callback_variant_ = callback; - } - std::unique_ptr create_ros_unique_ptr_from_ros_shared_ptr_message( const std::shared_ptr & message) @@ -487,12 +462,14 @@ class AnySubscriptionCallback } // Dispatch when input is a ros message and the output could be anything. - void + template + typename std::enable_if::value, + void>::type dispatch( std::shared_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), false); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -580,19 +557,19 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } // Dispatch when input is a serialized message and the output could be anything. void dispatch( - std::shared_ptr serialized_message, + std::shared_ptr serialized_message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), false); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -660,10 +637,10 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } void @@ -671,7 +648,7 @@ class AnySubscriptionCallback std::shared_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), true); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -790,10 +767,10 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } void @@ -801,7 +778,7 @@ class AnySubscriptionCallback std::unique_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), true); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -924,10 +901,10 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } constexpr @@ -965,9 +942,9 @@ class AnySubscriptionCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && callback) { - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(callback); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, static_cast(this), symbol); diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 43c7daa888..d8c529d56f 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -129,8 +129,7 @@ class CallbackGroup * added to the executor in either case. * * \param[in] group_type The type of the callback group. - * \param[in] get_node_context Lambda to retrieve the node context when - * checking that the creating node is valid and using the guard condition. + * \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. @@ -138,7 +137,7 @@ class CallbackGroup RCLCPP_PUBLIC explicit CallbackGroup( CallbackGroupType group_type, - std::function get_node_context, + rclcpp::Context::WeakPtr context, bool automatically_add_to_executor_with_node = true); /// Default destructor. @@ -185,18 +184,41 @@ class CallbackGroup * \return the number of entities in the callback group. */ RCLCPP_PUBLIC - size_t size() const; + size_t + size() const; + /// Return a reference to the 'can be taken' atomic boolean. + /** + * The resulting bool will be true in the case that no executor is currently + * using an executable entity from this group. + * The resulting bool will be false in the case that an executor is currently + * using an executable entity from this group, and the group policy doesn't + * allow a second take (eg mutual exclusion) + * \return a reference to the flag + */ RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); + /// Get the group type. + /** + * \return the group type + */ RCLCPP_PUBLIC const CallbackGroupType & type() const; + /// Collect all of the entity pointers contained in this callback group. + /** + * \param[in] sub_func Function to execute for each subscription + * \param[in] service_func Function to execute for each service + * \param[in] client_func Function to execute for each client + * \param[in] timer_func Function to execute for each timer + * \param[in] waitable_fuinc Function to execute for each waitable + */ RCLCPP_PUBLIC - void collect_all_ptrs( + void + collect_all_ptrs( std::function sub_func, std::function service_func, std::function client_func, @@ -228,16 +250,6 @@ class CallbackGroup bool automatically_add_to_executor_with_node() const; - /// Retrieve the guard condition used to signal changes to this callback group. - /** - * \param[in] context_ptr context to use when creating the guard condition - * \return guard condition if it is valid, otherwise nullptr. - */ - [[deprecated("Use get_notify_guard_condition() without arguments")]] - RCLCPP_PUBLIC - rclcpp::GuardCondition::SharedPtr - get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr); - /// Retrieve the guard condition used to signal changes to this callback group. /** * \return guard condition if it is valid, otherwise nullptr. @@ -297,7 +309,7 @@ class CallbackGroup std::shared_ptr notify_guard_condition_ = nullptr; std::recursive_mutex notify_guard_condition_mutex_; - std::function get_context_; + rclcpp::Context::WeakPtr context_; private: template diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f3346a067c..f69ab0b28f 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -20,13 +20,13 @@ #include #include #include -#include // NOLINT, cpplint doesn't think this is a cpp std header +#include #include #include #include #include #include -#include // NOLINT +#include #include #include "rcl/client.h" @@ -115,6 +115,29 @@ struct FutureAndRequestId /// Destructor. ~FutureAndRequestId() = default; }; + +template> +size_t +prune_requests_older_than_impl( + PendingRequestsT & pending_requests, + std::mutex & pending_requests_mutex, + std::chrono::time_point time_point, + std::vector * 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 @@ -363,12 +386,16 @@ class ClientBase std::shared_ptr 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 on_new_response_callback_{nullptr}; + // Declare client_handle_ after callback std::shared_ptr client_handle_; std::atomic in_use_by_wait_set_{false}; - - std::recursive_mutex callback_mutex_; - std::function on_new_response_callback_{nullptr}; }; template @@ -767,19 +794,11 @@ class Client : public ClientBase std::chrono::time_point time_point, std::vector * 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(); + return detail::prune_requests_older_than_impl( + pending_requests_, + pending_requests_mutex_, + time_point, + pruned_requests); } /// Configure client introspection. diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 45c70b9af2..4417f4d675 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include "rcl/context.h" #include "rcl/guard_condition.h" diff --git a/rclcpp/include/rclcpp/copy_all_parameter_values.hpp b/rclcpp/include/rclcpp/copy_all_parameter_values.hpp new file mode 100644 index 0000000000..cc61b621e1 --- /dev/null +++ b/rclcpp/include/rclcpp/copy_all_parameter_values.hpp @@ -0,0 +1,82 @@ +// 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 +#include + +#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 +void +copy_all_parameter_values( + const NodeT1 & source, const NodeT2 & destination, const bool override_existing_params = false) +{ + using Parameters = std::vector; + using Descriptions = std::vector; + 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 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_ diff --git a/rclcpp/include/rclcpp/create_generic_client.hpp b/rclcpp/include/rclcpp/create_generic_client.hpp new file mode 100644 index 0000000000..eade7bd9f1 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_client.hpp @@ -0,0 +1,90 @@ +// 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 +#include + +#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_base, + std::shared_ptr node_graph, + std::shared_ptr 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 +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_ diff --git a/rclcpp/include/rclcpp/create_generic_subscription.hpp b/rclcpp/include/rclcpp/create_generic_subscription.hpp index f5281cc673..c2549721b5 100644 --- a/rclcpp/include/rclcpp/create_generic_subscription.hpp +++ b/rclcpp/include/rclcpp/create_generic_subscription.hpp @@ -45,13 +45,15 @@ 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> +template< + typename CallbackT, + typename AllocatorT = std::allocator> std::shared_ptr create_generic_subscription( rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() ) @@ -60,13 +62,20 @@ std::shared_ptr create_generic_subscription( auto ts_lib = rclcpp::get_typesupport_library( topic_type, "rosidl_typesupport_cpp"); + auto allocator = options.get_allocator(); + + using rclcpp::AnySubscriptionCallback; + AnySubscriptionCallback + any_subscription_callback(*allocator); + any_subscription_callback.set(std::forward(callback)); + auto subscription = std::make_shared( topics_interface->get_node_base_interface(), std::move(ts_lib), topic_name, topic_type, qos, - callback, + any_subscription_callback, options); topics_interface->add_subscription(subscription, options.callback_group); diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 5b84930ff7..3a1e4b1a13 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -50,8 +50,8 @@ template< typename SubscriptionT, typename MessageMemoryStrategyT, typename NodeParametersT, - typename NodeTopicsT, - typename ROSMessageType = typename SubscriptionT::ROSMessageType> + typename NodeTopicsT +> typename std::shared_ptr 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> + std::shared_ptr subscription_topic_stats = nullptr; if (rclcpp::detail::resolve_enable_topic_statistics( @@ -80,8 +80,7 @@ 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> @@ -89,14 +88,14 @@ create_subscription( node_parameters, node_topics_interface, options.topic_stats_options.publish_topic, - qos); + options.topic_stats_options.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( + node_topics_interface->get_node_base_interface()->get_name(), publisher); std::weak_ptr< - rclcpp::topic_statistics::SubscriptionTopicStatistics + rclcpp::topic_statistics::SubscriptionTopicStatistics > weak_subscription_topic_stats(subscription_topic_stats); auto sub_call_back = [weak_subscription_topic_stats]() { auto subscription_topic_stats = weak_subscription_topic_stats.lock(); diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index d371466f0d..64d5b8e322 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -90,7 +90,8 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { return create_timer( clock, @@ -98,7 +99,8 @@ create_timer( std::forward(callback), group, node_base.get(), - node_timers.get()); + node_timers.get(), + autostart); } /// Create a timer with a given clock @@ -109,7 +111,8 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { return create_timer( clock, @@ -117,7 +120,8 @@ create_timer( std::forward(callback), group, rclcpp::node_interfaces::get_node_base_interface(node).get(), - rclcpp::node_interfaces::get_node_timers_interface(node).get()); + rclcpp::node_interfaces::get_node_timers_interface(node).get(), + autostart); } /// Convenience method to create a general timer with node resources. @@ -132,6 +136,7 @@ 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 @@ -144,7 +149,8 @@ create_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) { if (clock == nullptr) { throw std::invalid_argument{"clock cannot be null"}; @@ -160,7 +166,7 @@ create_timer( // Add a new generic timer. auto timer = rclcpp::GenericTimer::make_shared( - std::move(clock), period_ns, std::move(callback), node_base->get_context()); + std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart); node_timers->add_timer(timer, group); return timer; } @@ -187,7 +193,8 @@ create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) { if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; @@ -201,7 +208,7 @@ create_wall_timer( // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( - period_ns, std::move(callback), node_base->get_context()); + period_ns, std::move(callback), node_base->get_context(), autostart); node_timers->add_timer(timer, group); return timer; } diff --git a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp index e7196a1e11..234316a8f0 100644 --- a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp +++ b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp @@ -47,6 +47,11 @@ 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 diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 537e2a9bf6..6b48f441e3 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -18,6 +18,7 @@ #include #include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" #include "rcl/time.h" #include "rclcpp/visibility_control.hpp" @@ -158,6 +159,14 @@ class RCLCPP_PUBLIC Duration 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_ diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index 3f41de469c..887c571d16 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -117,12 +117,12 @@ class EventHandlerBase : public Waitable /// Add the Waitable to a wait set. RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// Check if the Waitable is ready. RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t & wait_set) override; /// Set a callback to be called when each new event instance occurs. /** @@ -260,6 +260,16 @@ class EventHandler : public EventHandlerBase } } + ~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 take_data() override @@ -284,7 +294,7 @@ class EventHandler : public EventHandlerBase /// Execute any entities of the Waitable that are ready. void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr & data) override { if (!data) { throw std::runtime_error("'data' is empty"); diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index ecb2a09905..b3a53373ed 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -206,6 +206,14 @@ class UnknownROSArgsError : public std::runtime_error const std::vector 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 { @@ -222,6 +230,14 @@ class EventNotRegisteredError : public std::runtime_error : 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 { diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2d5ca2149a..cfd7ea81fd 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -29,26 +29,24 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" +#include "rclcpp/executors/executor_notify_waitable.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/executor_options.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" #include "rclcpp/future_return_code.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set.hpp" namespace rclcpp { -typedef std::map> WeakCallbackGroupsToNodesMap; - // Forward declaration is used in convenience method signature. class Node; class ExecutorImplementation; @@ -282,21 +280,43 @@ class Executor void spin_node_some(std::shared_ptr node); - /// Collect work once and execute all available work, optionally within a duration. + /// Collect work once and execute all available work, optionally within a max 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 callbacks will cause this function - * to block (which may have unintended consequences). + * 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. * * \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 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 @@ -403,17 +423,6 @@ class Executor void cancel(); - /// Support dynamic switching of the memory strategy. - /** - * Switching the memory strategy while the executor is spinning in another threading could have - * unintended consequences. - * \param[in] memory_strategy Shared pointer to the memory strategy to set. - * \throws std::runtime_error if memory_strategy is null - */ - RCLCPP_PUBLIC - void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); - /// Returns true if the executor is currently spinning. /** * This function can be called asynchronously from any thread. @@ -498,6 +507,11 @@ class Executor static void execute_client(rclcpp::ClientBase::SharedPtr client); + /// Gather all of the waitable entities from associated nodes and callback groups. + RCLCPP_PUBLIC + void + collect_entities(); + /// Block until more work becomes avilable or timeout is reached. /** * Builds a set of waitable entities, which are passed to the middleware. @@ -509,62 +523,6 @@ class Executor void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Find node associated with a callback group - /** - * \param[in] weak_groups_to_nodes map of callback groups to nodes - * \param[in] group callback group to find assocatiated node - * \return Pointer to associated node if found, else nullptr - */ - RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group( - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group); - - /// Return true if the node has been added to this executor. - /** - * \param[in] node_ptr a shared pointer that points to a node base interface - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \return true if the node is associated with the executor, otherwise false - */ - RCLCPP_PUBLIC - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Find the callback group associated with a timer - /** - * \param[in] timer Timer to find associated callback group - * \return Pointer to callback group node if found, else nullptr - */ - RCLCPP_PUBLIC - rclcpp::CallbackGroup::SharedPtr - get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); - - /// Add a callback group to an executor - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - virtual void - add_callback_group_to_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - virtual void - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); - /// Check for executable in ready state and populate union structure. /** * \param[out] any_executable populated union structure of ready executable @@ -575,33 +533,6 @@ class Executor bool get_next_ready_executable(AnyExecutable & any_executable); - /// Check for executable in ready state and populate union structure. - /** - * This is the implementation of get_next_ready_executable that takes into - * account the current state of callback groups' association with nodes and - * executors. - * - * This checks in a particular order for available work: - * * Timers - * * Subscriptions - * * Services - * * Clients - * * Waitable - * - * If the next executable is not associated with this executor/node pair, - * then this method will return false. - * - * \param[out] any_executable populated union structure of ready executable - * \param[in] weak_groups_to_nodes mapping of callback groups to nodes - * \return true if an executable was ready and any_executable was populated, - * otherwise false - */ - RCLCPP_PUBLIC - bool - get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - /// Wait for executable in ready state and populate union structure. /** * If an executable is ready, it will return immediately, otherwise @@ -619,21 +550,6 @@ class Executor AnyExecutable & any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Add all callback groups that can be automatically added from associated nodes. - /** - * The executor, before collecting entities, verifies if any callback group from - * nodes associated with the executor, which is not already associated to an executor, - * can be automatically added to this executor. - * This takes care of any callback group that has been added to a node but not explicitly added - * to the executor. - * It is important to note that in order for the callback groups to be automatically added to an - * executor through this function, the node of the callback groups needs to have been added - * through the `add_node` method. - */ - RCLCPP_PUBLIC - virtual void - add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_); - /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; @@ -643,16 +559,8 @@ class Executor /// Guard condition for signaling the rmw layer to wake up for system shutdown. std::shared_ptr shutdown_guard_condition_; - /// Wait set for managing entities that the rmw layer waits on. - rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); - - // Mutex to protect the subsequent memory_strategy_. mutable std::mutex mutex_; - /// The memory strategy: an interface for handling user-defined memory allocation strategies. - memory_strategy::MemoryStrategy::SharedPtr - memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_); - /// The context associated with this executor. std::shared_ptr context_; @@ -662,39 +570,31 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); - typedef std::map> - WeakNodesToGuardConditionsMap; - - typedef std::map> - WeakCallbackGroupsToGuardConditionsMap; - - /// maps nodes to guard conditions - WeakNodesToGuardConditionsMap - weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Waitable containing guard conditions controlling the executor flow. + /** + * This waitable contains the interrupt and shutdown guard condition, as well + * as the guard condition associated with each node and callback group. + * By default, if any change is detected in the monitored entities, the notify + * waitable will awake the executor and rebuild the collections. + */ + std::shared_ptr notify_waitable_; - /// maps callback groups to guard conditions - WeakCallbackGroupsToGuardConditionsMap - weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::atomic_bool entities_need_rebuild_; - /// maps callback groups associated to nodes - WeakCallbackGroupsToNodesMap - weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Collector used to associate executable entities from nodes and guard conditions + rclcpp::executors::ExecutorEntitiesCollector collector_; - /// maps callback groups to nodes associated with executor - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// WaitSet to be waited on. + rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::optional> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - /// maps all callback groups to nodes - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Hold the current state of the collection being waited on by the waitset + rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); - /// nodes that are associated with the executor - std::list - weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Hold the current state of the notify waitable being waited on by the waitset + std::shared_ptr current_notify_waitable_ + RCPPUTILS_TSA_GUARDED_BY(mutex_); /// shutdown callback handle registered to Context rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index f7ff06055f..55a38709a7 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -29,6 +29,18 @@ 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 diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 166bb99119..517894a2a2 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -178,6 +178,12 @@ struct ExecutorEntitiesCollection /// Clear the entities collection void clear(); + + /// Remove entities that have expired weak ownership + /** + * \return The total number of removed entities + */ + size_t remove_expired_entities(); }; /// Build an entities collection from callback groups diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index eee8e59793..0de01fd4b0 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -48,11 +48,11 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable ~ExecutorNotifyWaitable() override = default; RCLCPP_PUBLIC - ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable(ExecutorNotifyWaitable & other); RCLCPP_PUBLIC - ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other); /// Add conditions to the wait set /** @@ -60,7 +60,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// Check conditions against the wait set /** @@ -69,7 +69,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t & wait_set) override; /// Perform work associated with the waitable. /** @@ -78,7 +78,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// Retrieve data to be used in the next execute call. /** diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp deleted file mode 100644 index f9fd2ff672..0000000000 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ /dev/null @@ -1,357 +0,0 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT 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__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ -#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ - -#include -#include -#include -#include -#include - -#include "rcl/guard_condition.h" -#include "rcl/wait.h" - -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace executors -{ -typedef std::map> WeakCallbackGroupsToNodesMap; - -class StaticExecutorEntitiesCollector final - : public rclcpp::Waitable, - public std::enable_shared_from_this -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector) - - // Constructor - RCLCPP_PUBLIC - StaticExecutorEntitiesCollector() = default; - - // Destructor - RCLCPP_PUBLIC - ~StaticExecutorEntitiesCollector(); - - /// Initialize StaticExecutorEntitiesCollector - /** - * \param p_wait_set A reference to the wait set to be used in the executor - * \param memory_strategy Shared pointer to the memory strategy to set. - * \throws std::runtime_error if memory strategy is null - */ - RCLCPP_PUBLIC - void - init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy); - - /// Finalize StaticExecutorEntitiesCollector to clear resources - RCLCPP_PUBLIC - bool - is_init() {return initialized_;} - - RCLCPP_PUBLIC - void - fini(); - - /// Execute the waitable. - RCLCPP_PUBLIC - void - execute(std::shared_ptr & data) override; - - /// Take the data so that it can be consumed with `execute`. - /** - * For `StaticExecutorEntitiesCollector`, this always return `nullptr`. - * \sa rclcpp::Waitable::take_data() - */ - RCLCPP_PUBLIC - std::shared_ptr - take_data() override; - - /// Function to add_handles_to_wait_set and wait for work and - /** - * block until the wait set is ready or until the timeout has been exceeded. - * \throws std::runtime_error if wait set couldn't be cleared or filled. - * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error() - */ - RCLCPP_PUBLIC - void - refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - - /** - * \throws std::runtime_error if it couldn't add guard condition to wait set - */ - RCLCPP_PUBLIC - void - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - size_t - get_number_of_ready_guard_conditions() override; - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - * \return boolean whether the node from the callback group is new - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - bool - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group_from_map - */ - RCLCPP_PUBLIC - bool - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /** - * \see rclcpp::Executor::add_node() - * \throw std::runtime_error if node was already added - */ - RCLCPP_PUBLIC - bool - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /** - * \see rclcpp::Executor::remove_node() - * \throw std::runtime_error if no guard condition is associated with node. - */ - RCLCPP_PUBLIC - bool - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes(); - - /// Complete all available queued work without blocking. - /** - * This function checks if after the guard condition was triggered - * (or a spurious wakeup happened) we are really ready to execute - * i.e. re-collect entities - */ - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - /// Return number of timers - /** - * \return number of timers - */ - RCLCPP_PUBLIC - size_t - get_number_of_timers() {return exec_list_.number_of_timers;} - - /// Return number of subscriptions - /** - * \return number of subscriptions - */ - RCLCPP_PUBLIC - size_t - get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;} - - /// Return number of services - /** - * \return number of services - */ - RCLCPP_PUBLIC - size_t - get_number_of_services() {return exec_list_.number_of_services;} - - /// Return number of clients - /** - * \return number of clients - */ - RCLCPP_PUBLIC - size_t - get_number_of_clients() {return exec_list_.number_of_clients;} - - /// Return number of waitables - /** - * \return number of waitables - */ - RCLCPP_PUBLIC - size_t - get_number_of_waitables() {return exec_list_.number_of_waitables;} - - /** Return a SubscritionBase Sharedptr by index. - * \param[in] i The index of the SubscritionBase - * \return a SubscritionBase shared pointer - * \throws std::out_of_range if the argument is higher than the size of the structrue. - */ - RCLCPP_PUBLIC - rclcpp::SubscriptionBase::SharedPtr - get_subscription(size_t i) {return exec_list_.subscription[i];} - - /** Return a TimerBase Sharedptr by index. - * \param[in] i The index of the TimerBase - * \return a TimerBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::TimerBase::SharedPtr - get_timer(size_t i) {return exec_list_.timer[i];} - - /** Return a ServiceBase Sharedptr by index. - * \param[in] i The index of the ServiceBase - * \return a ServiceBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ServiceBase::SharedPtr - get_service(size_t i) {return exec_list_.service[i];} - - /** Return a ClientBase Sharedptr by index - * \param[in] i The index of the ClientBase - * \return a ClientBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ClientBase::SharedPtr - get_client(size_t i) {return exec_list_.client[i];} - - /** Return a Waitable Sharedptr by index - * \param[in] i The index of the Waitable - * \return a Waitable shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::Waitable::SharedPtr - get_waitable(size_t i) {return exec_list_.waitable[i];} - -private: - /// Function to reallocate space for entities in the wait set. - /** - * \throws std::runtime_error if wait set couldn't be cleared or resized. - */ - void - prepare_wait_set(); - - void - fill_executable_list(); - - void - fill_memory_strategy(); - - /// Return true if the node belongs to the collector - /** - * \param[in] node_ptr a node base interface shared pointer - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \return boolean whether a node belongs the collector - */ - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Add all callback groups that can be automatically added by any executor - /// and is not already associated with an executor from nodes - /// that are associated with executor - /** - * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor() - */ - void - add_callback_groups_from_nodes_associated_to_executor(); - - void - fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Memory strategy: an interface for handling user-defined memory allocation strategies. - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; - - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_; - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; - - typedef std::map> - WeakNodesToGuardConditionsMap; - WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; - - /// List of weak nodes registered in the static executor - std::list weak_nodes_; - - // Mutex to protect vector of new nodes. - std::mutex new_nodes_mutex_; - std::vector new_nodes_; - - /// Wait set for managing entities that the rmw layer waits on. - rcl_wait_set_t * p_wait_set_ = nullptr; - - /// Executable list: timers, subscribers, clients, services and waitables - rclcpp::experimental::ExecutableList exec_list_; - - /// Bool to check if the entities collector has been initialized - bool initialized_ = false; -}; - -} // namespace executors -} // namespace rclcpp - -#endif // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 5294605eaf..6f22909caf 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -15,24 +15,13 @@ #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ +#include #include -#include -#include #include -#include -#include - -#include "rmw/rmw.h" #include "rclcpp/executor.hpp" -#include "rclcpp/executors/static_executor_entities_collector.hpp" -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/rate.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp/visibility_control.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" namespace rclcpp { @@ -65,7 +54,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor explicit StaticSingleThreadedExecutor( const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); - /// Default destrcutor. + /// Default destructor. RCLCPP_PUBLIC virtual ~StaticSingleThreadedExecutor(); @@ -116,105 +105,31 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor void spin_all(std::chrono::nanoseconds max_duration) 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; - - /// 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::StaticSingleThreadedExecutor::add_node - */ - RCLCPP_PUBLIC - void - add_node(std::shared_ptr 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 node_ptr, bool notify = true) override; - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - 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 - get_automatically_added_callback_groups_from_nodes() override; - protected: /** * @brief Executes ready executables from wait set. + * @param collection entities to evaluate for ready executables. + * @param wait_result result to check for ready executables. * @param spin_once if true executes only the first ready executable. * @return true if any executable was ready. */ - RCLCPP_PUBLIC bool - execute_ready_executables(bool spin_once = false); + execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once); - RCLCPP_PUBLIC void spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); - RCLCPP_PUBLIC void spin_once_impl(std::chrono::nanoseconds timeout) override; + std::optional> + collect_and_wait(std::chrono::nanoseconds timeout); + private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) - - StaticExecutorEntitiesCollector::SharedPtr entities_collector_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp index 1e5346116a..1d50b1659e 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#include + namespace rclcpp { namespace experimental @@ -31,8 +33,11 @@ class BufferImplementationBase virtual BufferT dequeue() = 0; virtual void enqueue(BufferT request) = 0; + virtual std::vector get_all_data() = 0; + virtual void clear() = 0; virtual bool has_data() const = 0; + virtual size_t available_capacity() const = 0; }; } // namespace buffers diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index 05092bb23b..268c3f6649 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" @@ -44,6 +45,7 @@ class IntraProcessBufferBase virtual bool has_data() const = 0; virtual bool use_take_shared_method() const = 0; + virtual size_t available_capacity() const = 0; }; template< @@ -65,6 +67,9 @@ class IntraProcessBuffer : public IntraProcessBufferBase virtual MessageSharedPtr consume_shared() = 0; virtual MessageUniquePtr consume_unique() = 0; + + virtual std::vector get_all_data_shared() = 0; + virtual std::vector get_all_data_unique() = 0; }; template< @@ -95,7 +100,7 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(buffer_.get()), static_cast(this)); @@ -128,6 +133,16 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(); } + std::vector get_all_data_shared() override + { + return get_all_data_shared_impl(); + } + + std::vector get_all_data_unique() override + { + return get_all_data_unique_impl(); + } + bool has_data() const override { return buffer_->has_data(); @@ -143,6 +158,11 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer::value; } + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + private: std::unique_ptr> buffer_; @@ -237,6 +257,71 @@ class TypedIntraProcessBuffer : public IntraProcessBufferdequeue(); } + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_shared_impl() + { + return buffer_->get_all_data(); + } + + // MessageUniquePtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_shared_impl() + { + std::vector 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 std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_unique_impl() + { + std::vector 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(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 std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_unique_impl() + { + return buffer_->get_all_data(); + } }; } // namespace buffers diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index 2c06ea6cbe..b8fe79a5ff 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#include #include #include #include @@ -52,7 +53,10 @@ class RingBufferImplementation : public BufferImplementationBase if (capacity == 0) { throw std::invalid_argument("capacity must be a positive, non-zero value"); } - TRACEPOINT(rclcpp_construct_ring_buffer, static_cast(this), capacity_); + TRACETOOLS_TRACEPOINT( + rclcpp_construct_ring_buffer, + static_cast(this), + capacity_); } virtual ~RingBufferImplementation() {} @@ -63,13 +67,13 @@ class RingBufferImplementation : public BufferImplementationBase * * \param request the element to be stored in the ring buffer */ - void enqueue(BufferT request) + void enqueue(BufferT request) override { std::lock_guard lock(mutex_); write_index_ = next_(write_index_); ring_buffer_[write_index_] = std::move(request); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ring_buffer_enqueue, static_cast(this), write_index_, @@ -89,7 +93,7 @@ class RingBufferImplementation : public BufferImplementationBase * * \return the element that is being removed from the ring buffer */ - BufferT dequeue() + BufferT dequeue() override { std::lock_guard lock(mutex_); @@ -98,7 +102,7 @@ class RingBufferImplementation : public BufferImplementationBase } auto request = std::move(ring_buffer_[read_index_]); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ring_buffer_dequeue, static_cast(this), read_index_, @@ -110,6 +114,17 @@ class RingBufferImplementation : public BufferImplementationBase 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 get_all_data() override + { + return get_all_data_impl(); + } + /// Get the next index value for the ring buffer /** * This member function is thread-safe. @@ -129,7 +144,7 @@ class RingBufferImplementation : public BufferImplementationBase * * \return `true` if there is data and `false` otherwise */ - inline bool has_data() const + inline bool has_data() const override { std::lock_guard lock(mutex_); return has_data_(); @@ -148,9 +163,21 @@ class RingBufferImplementation : public BufferImplementationBase return is_full_(); } - void clear() + /// 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 { - TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); + std::lock_guard lock(mutex_); + return available_capacity_(); + } + + void clear() override + { + TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); } private: @@ -189,6 +216,82 @@ class RingBufferImplementation : public BufferImplementationBase 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 + struct is_std_unique_ptr final : std::false_type {}; + template + struct is_std_unique_ptr> 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::value && + std::is_copy_constructible< + typename is_std_unique_ptr::Ptr_type + >::value, + void> * = nullptr> + std::vector get_all_data_impl() + { + std::lock_guard lock(mutex_); + std::vector result_vtr; + result_vtr.reserve(size_); + for (size_t id = 0; id < size_; ++id) { + result_vtr.emplace_back( + new typename is_std_unique_ptr::Ptr_type( + *(ring_buffer_[(read_index_ + id) % capacity_]))); + } + return result_vtr; + } + + template::value, void> * = nullptr> + std::vector get_all_data_impl() + { + std::lock_guard lock(mutex_); + std::vector 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::value && + !std::is_copy_constructible::value, void> * = nullptr> + std::vector get_all_data_impl() + { + throw std::logic_error("Underlined type results in invalid get_all_data_impl()"); + return {}; + } + + template::value && + !std::is_copy_constructible::Ptr_type>::value, + void> * = nullptr> + std::vector 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 ring_buffer_; diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp index 148ede66ea..dd5b1ebe63 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -243,6 +243,11 @@ class EventsExecutor : public rclcpp::Executor std::function 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::EntitySharedPtr @@ -269,9 +274,12 @@ class EventsExecutor : public rclcpp::Executor rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_; std::shared_ptr entities_collector_; - std::shared_ptr current_entities_collection_; std::shared_ptr notify_waitable_; + /// Mutex to protect the current_entities_collection_ + std::recursive_mutex collection_mutex_; + std::shared_ptr current_entities_collection_; + /// Flag used to reduce the number of unnecessary waitable events std::atomic notify_waitable_event_pushed_ {false}; diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 11f2dda6a4..ffd77cb99c 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -28,6 +28,7 @@ #include #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" @@ -112,9 +113,40 @@ class IntraProcessManager * \param subscription the SubscriptionIntraProcess to register. * \return an unsigned 64-bit integer which is the subscription's unique id. */ - RCLCPP_PUBLIC + template< + typename ROSMessageType, + typename Alloc = std::allocator + > uint64_t - add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription); + add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription) + { + std::unique_lock 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( + pub_id, sub_id, + subscription->use_take_shared_method()); + } + } + } + + return sub_id; + } /// Unregister a subscription using the subscription's unique id. /** @@ -131,14 +163,21 @@ class IntraProcessManager * 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); + add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer = + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr()); /// Unregister a publisher using the publisher's unique id. /** @@ -292,6 +331,34 @@ class IntraProcessManager } } + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_shared_msg_to_buffer( + std::shared_ptr message, + uint64_t subscription_id) + { + add_shared_msg_to_buffers(message, {subscription_id}); + } + + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_owned_msg_to_buffer( + std::unique_ptr message, + uint64_t subscription_id, + typename allocator::AllocRebind::allocator_type & allocator) + { + add_owned_msg_to_buffers( + std::move(message), {subscription_id}, allocator); + } + /// Return true if the given rmw_gid_t matches any stored Publishers. RCLCPP_PUBLIC bool @@ -306,6 +373,11 @@ class IntraProcessManager 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 { @@ -319,6 +391,9 @@ class IntraProcessManager using PublisherMap = std::unordered_map; + using PublisherBufferMap = + std::unordered_map; + using PublisherToSubscriptionIdsMap = std::unordered_map; @@ -337,6 +412,54 @@ class IntraProcessManager rclcpp::PublisherBase::SharedPtr pub, rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const; + template< + typename ROSMessageType, + typename Alloc = std::allocator + > + 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; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + 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 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, @@ -462,7 +585,7 @@ class IntraProcessManager auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter))); + subscription->provide_intra_process_data(MessageUniquePtr(ptr, deleter)); } continue; @@ -481,13 +604,13 @@ class IntraProcessManager "subscription use different allocator types, which is not supported"); } - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageTypeAllocator ros_message_alloc(allocator); - auto ptr = ros_message_alloc.allocate(1); - ros_message_alloc.construct(ptr); + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr); ROSMessageTypeDeleter deleter; allocator::set_allocator_for_deleter(&deleter, &allocator); - rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); auto ros_msg = std::unique_ptr(ptr, deleter); ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { @@ -505,7 +628,7 @@ class IntraProcessManager MessageAllocTraits::construct(allocator, ptr, *message); ros_message_subscription->provide_intra_process_message( - std::move(MessageUniquePtr(ptr, deleter))); + MessageUniquePtr(ptr, deleter)); } } } @@ -515,6 +638,7 @@ class IntraProcessManager PublisherToSubscriptionIdsMap pub_to_subs_; SubscriptionMap subscriptions_; PublisherMap publishers_; + PublisherBufferMap publisher_buffers_; mutable std::shared_timed_mutex mutex_; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index ec89ebc5ef..5c562a61ff 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -87,7 +87,7 @@ class SubscriptionIntraProcess buffer_type), any_callback_(callback) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), static_cast(&any_callback_)); @@ -132,7 +132,7 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { execute_impl(data); } @@ -140,15 +140,14 @@ class SubscriptionIntraProcess protected: template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr &) { - (void)data; throw std::runtime_error("Subscription intra-process can't handle serialized messages"); } template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr & data) { if (!data) { return; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 6583e74ae7..74792e8751 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -60,10 +60,19 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + 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; + is_ready(const rcl_wait_set_t & wait_set) override = 0; std::shared_ptr take_data() override = 0; @@ -76,7 +85,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override = 0; + execute(const std::shared_ptr & data) override = 0; virtual bool diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 30debed83a..2f384f351c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -30,6 +30,7 @@ #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "tracetools/tracetools.h" @@ -93,14 +94,23 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_type, qos_profile, std::make_shared(subscribed_type_allocator_)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ipb_to_subscription, static_cast(buffer_.get()), static_cast(this)); } + void + add_to_wait_set(rcl_wait_set_t & wait_set) override + { + if (this->buffer_->has_data()) { + this->trigger_guard_condition(); + } + detail::add_guard_condition_to_rcl_wait_set(wait_set, this->gc_); + } + bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t & wait_set) override { (void) wait_set; return buffer_->has_data(); @@ -169,6 +179,11 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff return buffer_->use_take_shared_method(); } + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + protected: void trigger_guard_condition() override diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp index e8830c01a0..197397e8b8 100644 --- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -22,10 +22,10 @@ #include #include #include +#include #include #include #include - #include "rclcpp/context.hpp" #include "rclcpp/timer.hpp" @@ -172,13 +172,14 @@ class TimersManager * @brief Get the amount of time before the next timer triggers. * This function is thread safe. * - * @return std::chrono::nanoseconds to wait, + * @return std::optional 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::chrono::nanoseconds get_head_timeout(); + std::optional get_head_timeout(); private: RCLCPP_DISABLE_COPY(TimersManager) @@ -512,12 +513,13 @@ class TimersManager * @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::chrono::nanoseconds to wait, + * @return std::optional 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::chrono::nanoseconds get_head_timeout_unsafe(); + std::optional get_head_timeout_unsafe(); /** * @brief Executes all the timers currently ready when the function is invoked @@ -527,7 +529,7 @@ class TimersManager void execute_ready_timers_unsafe(); // Callback to be called when timer is ready - std::function on_ready_callback_ = nullptr; + std::function on_ready_callback_; // Thread used to run the timers execution task std::thread timers_thread_; diff --git a/rclcpp/include/rclcpp/generic_client.hpp b/rclcpp/include/rclcpp/generic_client.hpp new file mode 100644 index 0000000000..d6073decfc --- /dev/null +++ b/rclcpp/include/rclcpp/generic_client.hpp @@ -0,0 +1,207 @@ +// 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 +#include +#include +#include +#include +#include + +#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; + + using Promise = std::promise; + using SharedPromise = std::shared_ptr; + + using Future = std::future; + using SharedFuture = std::shared_future; + + RCLCPP_SMART_PTR_DEFINITIONS(GenericClient) + + /// A convenient GenericClient::Future and request id pair. + /** + * Public members: + * - future: a std::future. + * - 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 + { + using detail::FutureAndRequestId::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 + create_request_header() override; + + RCLCPP_PUBLIC + void + handle_response( + std::shared_ptr request_header, + std::shared_ptr 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> + size_t + prune_requests_older_than( + std::chrono::time_point time_point, + std::vector * 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>; // Use variant for extension + + int64_t + async_send_request_impl( + const Request request, + CallbackInfoVariant value); + + std::optional + get_and_erase_pending_request( + int64_t request_number); + + RCLCPP_DISABLE_COPY(GenericClient) + + std::map, + CallbackInfoVariant>> pending_requests_; + std::mutex pending_requests_mutex_; + +private: + std::shared_ptr ts_lib_; + const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_; +}; +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp index 7cd2d8bc39..292e6900d3 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -77,7 +77,7 @@ class GenericPublisher : public rclcpp::PublisherBase : rclcpp::PublisherBase( node_base, topic_name, - *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), options.template to_rcl_publisher_options(qos), // NOTE(methylDragon): Passing these args separately is necessary for event binding options.event_callbacks, diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 975a9d0d0d..dd0e8be94d 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -74,20 +74,32 @@ class GenericSubscription : public rclcpp::SubscriptionBase const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - // TODO(nnmm): Add variant for callback with message info. See issue #1604. - std::function)> callback, + AnySubscriptionCallback callback, const rclcpp::SubscriptionOptionsWithAllocator & options) : SubscriptionBase( node_base, - *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + *rclcpp::get_message_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), + any_callback_(callback), ts_lib_(ts_lib) - {} + { + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + static_cast(&any_callback_)); + +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } RCLCPP_PUBLIC virtual ~GenericSubscription() = default; @@ -150,8 +162,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase private: RCLCPP_DISABLE_COPY(GenericSubscription) - - std::function)> callback_; + AnySubscriptionCallback> any_callback_; // The type support library should stay loaded, so it is stored in the GenericSubscription std::shared_ptr ts_lib_; }; diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 350f306010..594234657c 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -48,7 +48,7 @@ class GuardCondition */ RCLCPP_PUBLIC explicit GuardCondition( - rclcpp::Context::SharedPtr context = + const 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,11 +57,6 @@ class GuardCondition 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 & @@ -105,7 +100,7 @@ class GuardCondition */ RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set); + add_to_wait_set(rcl_wait_set_t & wait_set); /// Set a callback to be called whenever the guard condition is triggered. /** @@ -128,13 +123,14 @@ class GuardCondition set_on_trigger_callback(std::function callback); protected: - rclcpp::Context::SharedPtr context_; rcl_guard_condition_t rcl_guard_condition_; std::atomic in_use_by_wait_set_{false}; std::recursive_mutex reentrant_mutex_; std::function on_trigger_callback_{nullptr}; size_t unread_count_{0}; - rcl_wait_set_t * wait_set_{nullptr}; + // the type of wait_set_ is actually rcl_wait_set_t *, but it's never + // dereferenced, only compared to, so make it void * to avoid accidental use + void * wait_set_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index 77f7f8d670..3b8e8a1625 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -126,9 +126,6 @@ class Logger std::shared_ptr> 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 diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7ecb67e9b1..35863abba4 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -42,6 +42,7 @@ #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" @@ -56,6 +57,7 @@ #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" @@ -232,13 +234,15 @@ class Node : public std::enable_shared_from_this * \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 rclcpp::WallTimer::SharedPtr create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true); /// Create a timer that uses the node clock to drive the callback. /** @@ -317,6 +321,22 @@ class Node : public std::enable_shared_from_this 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 @@ -355,12 +375,14 @@ class Node : public std::enable_shared_from_this * `%callback_group`. * \return Shared pointer to the created generic subscription. */ - template> + template< + typename CallbackT, + typename AllocatorT = std::allocator> std::shared_ptr create_generic_subscription( const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() ) @@ -969,7 +991,16 @@ class Node : public std::enable_shared_from_this /// Return a list of parameters with any of the given prefixes, up to the given depth. /** - * \todo: properly document and test this method. + * 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. */ RCLCPP_PUBLIC rcl_interfaces::msg::ListParametersResult @@ -1302,6 +1333,26 @@ class Node : public std::enable_shared_from_this 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 @@ -1454,6 +1505,11 @@ class Node : public std::enable_shared_from_this 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 @@ -1586,11 +1642,18 @@ class Node : public std::enable_shared_from_this 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 hidden_impl_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d6b090d4d1..d55a23f9c1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -110,14 +110,16 @@ typename rclcpp::WallTimer::SharedPtr Node::create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + bool autostart) { return rclcpp::create_wall_timer( period, std::move(callback), group, this->node_base_.get(), - this->node_timers_.get()); + this->node_timers_.get(), + autostart); } template @@ -219,13 +221,13 @@ Node::create_generic_publisher( ); } -template +template std::shared_ptr Node::create_generic_subscription( const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options) { return rclcpp::create_generic_subscription( @@ -233,7 +235,7 @@ Node::create_generic_subscription( extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), topic_type, qos, - std::move(callback), + std::forward(callback), options ); } diff --git a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp index b999344ca9..a243e9611a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp @@ -167,6 +167,7 @@ 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 \ @@ -189,7 +190,7 @@ init_tuple(NodeT & n) /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \ template \ explicit NodeInterfacesSupports(ArgsT && ... args) \ - : NodeInterfacesSupports( \ + : NodeInterfacesSupports( \ std::forward(args) ...) \ {} \ \ @@ -200,6 +201,7 @@ init_tuple(NodeT & n) } \ }; \ } // namespace rclcpp::node_interfaces::detail +// *INDENT-ON* } // namespace detail } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index d167515784..863dbee1bf 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -113,6 +113,14 @@ class NodeGraph : public NodeGraphInterface 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; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 5967997ac7..80abc308c1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -305,6 +305,24 @@ class NodeGraphInterface 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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index bb4886d592..80a20fb4a3 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -30,6 +30,7 @@ rclcpp::node_interfaces::NodeTimeSourceInterface, \ rclcpp::node_interfaces::NodeTimersInterface, \ rclcpp::node_interfaces::NodeTopicsInterface, \ + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \ rclcpp::node_interfaces::NodeWaitablesInterface @@ -118,6 +119,7 @@ class NodeInterfaces * - 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 @@ -125,7 +127,9 @@ class NodeInterfaces * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro. * * Usage example: - * ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)``` + * ```cpp + * 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: diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp new file mode 100644 index 0000000000..8aa563bba2 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -0,0 +1,63 @@ +// 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 + +#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 impl_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp new file mode 100644 index 0000000000..e7e0b0af2e --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp @@ -0,0 +1,44 @@ +// 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_ diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index a30ce4cf11..71b0d997cf 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -43,6 +43,7 @@ class NodeOptions * - arguments = {} * - parameter_overrides = {} * - use_global_arguments = true + * - enable_rosout = true * - use_intra_process_comms = false * - enable_topic_statistics = false * - start_parameter_services = true diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp index 3aa70d8a85..6960a1bccf 100644 --- a/rclcpp/include/rclcpp/parameter_events_filter.hpp +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -48,7 +48,7 @@ class ParameterEventsFilter * * Example Usage: * - * If you have recieved a parameter event and are only interested in parameters foo and + * If you have received a parameter event and are only interested in parameters foo and * bar being added or changed but don't care about deletion. * * ```cpp diff --git a/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp index f74c36a866..fac896ea46 100644 --- a/rclcpp/include/rclcpp/parameter_value.hpp +++ b/rclcpp/include/rclcpp/parameter_value.hpp @@ -24,6 +24,7 @@ #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 diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f474a67192..c6f16643c6 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -32,6 +32,9 @@ #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" @@ -109,6 +112,12 @@ class Publisher : public PublisherBase [[deprecated("use std::shared_ptr")]] = std::shared_ptr; + using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + ROSMessageType, + ROSMessageTypeAllocator, + ROSMessageTypeDeleter + >::SharedPtr; + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) /// Default constructor. @@ -171,11 +180,14 @@ class Publisher : public PublisherBase throw std::invalid_argument( "intraprocess communication is not allowed with a zero qos history depth value"); } - if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) { - throw std::invalid_argument( - "intraprocess communication allowed only with volatile durability"); + 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(ros_message_type_allocator_)); } - uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this()); + uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_); this->setup_intra_process( intra_process_publisher_id, ipm); @@ -242,9 +254,18 @@ class Publisher : public PublisherBase 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 { - this->do_intra_process_ros_message_publish(std::move(msg)); + 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)); + } } } @@ -269,8 +290,8 @@ class Publisher : public PublisherBase { // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(msg); + this->do_inter_process_publish(msg); + return; } // 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. @@ -297,26 +318,31 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { // In this case we're not using intra process. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); - return this->do_inter_process_publish(ros_msg); + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; } bool inter_process_publish_needed = get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - 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::convert_to_ros_message(*msg, ros_msg); + auto ros_msg_ptr = std::make_shared(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); this->do_intra_process_publish(std::move(msg)); - this->do_inter_process_publish(ros_msg); + this->do_inter_process_publish(*ros_msg_ptr); + if (buffer_) { + buffer_->add_shared(ros_msg_ptr); + } } else { + if (buffer_) { + auto ros_msg_ptr = std::make_shared(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + buffer_->add_shared(ros_msg_ptr); + } this->do_intra_process_publish(std::move(msg)); } } @@ -339,13 +365,12 @@ class Publisher : public PublisherBase > 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. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(msg, ros_msg); - // In this case we're not using intra process. - return this->do_inter_process_publish(ros_msg); + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; } // Otherwise we have to allocate memory in a unique_ptr and pass it along. @@ -390,7 +415,7 @@ class Publisher : public PublisherBase 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(std::move(loaned_msg.release())); + this->do_loaned_message_publish(loaned_msg.release()); } else { // we don't release the ownership, let the middleware copy the ros message // and thus the destructor of rclcpp::LoanedMessage cleans up the memory. @@ -421,7 +446,7 @@ class Publisher : public PublisherBase void do_inter_process_publish(const ROSMessageType & msg) { - TRACEPOINT(rclcpp_publish, nullptr, static_cast(&msg)); + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(&msg)); auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { @@ -456,6 +481,7 @@ class Publisher : public PublisherBase do_loaned_message_publish( std::unique_ptr> msg) { + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(msg.get())); auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { @@ -484,7 +510,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -506,7 +532,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -529,7 +555,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -580,6 +606,8 @@ class Publisher : public PublisherBase PublishedTypeDeleter published_type_deleter_; ROSMessageTypeAllocator ros_message_type_allocator_; ROSMessageTypeDeleter ros_message_type_deleter_; + + BufferSharedPtr buffer_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index d9181bea43..9a6c398eeb 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -139,6 +139,12 @@ class PublisherBase : public std::enable_shared_from_this 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 @@ -215,6 +221,17 @@ class PublisherBase : public std::enable_shared_from_this std::vector 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 diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 3501834dd1..01fd314f49 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -24,6 +24,7 @@ #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" @@ -40,6 +41,9 @@ 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; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp deleted file mode 100644 index efc0aa5739..0000000000 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ /dev/null @@ -1,22 +0,0 @@ -// 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_ diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 55d3bbcb85..5b04b72b81 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -19,6 +19,8 @@ #include #include +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -31,9 +33,20 @@ 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; }; @@ -42,14 +55,13 @@ using std::chrono::duration_cast; using std::chrono::nanoseconds; template -class GenericRate : public RateBase +class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase { public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) explicit GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0 / rate))) + : period_(duration_cast(duration(1.0 / rate))), last_interval_(Clock::now()) {} explicit GenericRate(std::chrono::nanoseconds period) : period_(period), last_interval_(Clock::now()) @@ -87,12 +99,18 @@ class GenericRate : public RateBase 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() { @@ -112,8 +130,59 @@ class GenericRate : public RateBase std::chrono::time_point last_interval_; }; -using Rate = GenericRate; -using WallRate = GenericRate; +class Rate : public RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Rate) + + RCLCPP_PUBLIC + explicit Rate( + const double rate, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + + RCLCPP_PUBLIC + explicit Rate( + const Duration & period, + Clock::SharedPtr clock = std::make_shared(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); +}; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index ef587578e2..50af3f1a89 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -54,6 +54,7 @@ * - rclcpp::ParameterValue * - rclcpp::AsyncParametersClient * - rclcpp::SyncParametersClient + * - rclcpp::copy_all_parameter_values() * - rclcpp/parameter.hpp * - rclcpp/parameter_value.hpp * - rclcpp/parameter_client.hpp @@ -95,6 +96,9 @@ * - 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: * @@ -164,6 +168,7 @@ #include #include +#include "rclcpp/copy_all_parameter_values.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 3cfc11e9ca..9e08dc235d 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -265,15 +265,19 @@ class ServiceBase std::shared_ptr 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 on_new_request_callback_{nullptr}; + // Declare service_handle_ after callback std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; rclcpp::Logger node_logger_; std::atomic in_use_by_wait_set_{false}; - - std::recursive_mutex callback_mutex_; - std::function on_new_request_callback_{nullptr}; }; template @@ -348,7 +352,7 @@ class Service rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -383,7 +387,7 @@ class Service } service_handle_ = service_handle; - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -420,7 +424,7 @@ class Service // In this case, rcl owns the service handle memory service_handle_ = std::shared_ptr(new rcl_service_t); service_handle_->impl = service_handle->impl; - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -482,6 +486,14 @@ class Service { 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"); } diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 46379744a1..28e929c495 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -121,7 +121,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } for (size_t i = 0; i < waitable_handles_.size(); ++i) { - if (waitable_handles_[i]->is_ready(wait_set)) { + if (waitable_handles_[i]->is_ready(*wait_set)) { waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i])); } } @@ -235,7 +235,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (const std::shared_ptr & waitable : waitable_handles_) { - waitable->add_to_wait_set(wait_set); + waitable->add_to_wait_set(*wait_set); } return true; } diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp index 0e7d4366e5..703066fa3f 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -15,10 +15,17 @@ #ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ #define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ +#include +#include #include +#include +#include +#include #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" @@ -50,13 +57,24 @@ class MessagePoolMemoryStrategy public: RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy) - /// Default constructor MessagePoolMemoryStrategy() - : next_array_index_(0) { + pool_mutex_ = std::make_shared(); + + pool_ = std::shared_ptr>( + new std::array, + [](std::array * arr) { + for (size_t i = 0; i < Size; ++i) { + free((*arr)[i]); + } + delete arr; + }); + + free_list_ = std::make_shared>(); + for (size_t i = 0; i < Size; ++i) { - pool_[i].msg_ptr_ = std::make_shared(); - pool_[i].used = false; + (*pool_)[i] = static_cast(malloc(sizeof(MessageT))); + free_list_->push_back(i); } } @@ -68,43 +86,85 @@ class MessagePoolMemoryStrategy */ std::shared_ptr borrow_message() { - size_t current_index = next_array_index_; - next_array_index_ = (next_array_index_ + 1) % Size; - if (pool_[current_index].used) { - throw std::runtime_error("Tried to access message that was still in use! Abort."); + std::lock_guard lock(*pool_mutex_); + if (free_list_->size() == 0) { + throw std::runtime_error("No more free slots in the pool"); } - pool_[current_index].msg_ptr_->~MessageT(); - new (pool_[current_index].msg_ptr_.get())MessageT; - pool_[current_index].used = true; - return pool_[current_index].msg_ptr_; + size_t current_index = free_list_->pop_front(); + + return std::shared_ptr( + new((*pool_)[current_index]) MessageT(), + [pool = this->pool_, pool_mutex = this->pool_mutex_, + free_list = this->free_list_](MessageT * p) { + std::lock_guard lock(*pool_mutex); + for (size_t i = 0; i < Size; ++i) { + if ((*pool)[i] == p) { + p->~MessageT(); + free_list->push_back(i); + break; + } + } + }); } /// Return a message to the message pool. /** - * Manage metadata in the message pool ring buffer to release the message. + * This does nothing since the message isn't returned to the pool until the user has dropped + * all references. * \param[in] msg Shared pointer to the message to return. */ void return_message(std::shared_ptr & msg) { - for (size_t i = 0; i < Size; ++i) { - if (pool_[i].msg_ptr_ == msg) { - pool_[i].used = false; - return; - } - } - throw std::runtime_error("Unrecognized message ptr in return_message."); + (void)msg; } protected: - struct PoolMember + template + class CircularArray { - std::shared_ptr msg_ptr_; - bool used; +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 array_; }; - std::array pool_; - size_t next_array_index_; + // 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 pool_mutex_; + std::shared_ptr> pool_; + std::shared_ptr> free_list_; }; } // namespace message_pool_memory_strategy diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index eb1a980dd3..f8a10a5c27 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -104,7 +104,7 @@ class Subscription : public SubscriptionBase private: using SubscriptionTopicStatisticsSharedPtr = - std::shared_ptr>; + std::shared_ptr; public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -127,6 +127,7 @@ class Subscription : public SubscriptionBase * 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, @@ -148,6 +149,7 @@ class Subscription : public SubscriptionBase 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)) { @@ -163,10 +165,6 @@ class Subscription : public SubscriptionBase 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, @@ -185,7 +183,7 @@ class Subscription : public SubscriptionBase this->get_topic_name(), // important to get like this, as it has the fully-qualified name qos_profile, resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), static_cast(subscription_intra_process_.get())); @@ -193,7 +191,8 @@ class Subscription : public SubscriptionBase // Add it to the intra process manager. using rclcpp::experimental::IntraProcessManager; auto ipm = context->get_sub_context(); - uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process_); + uint64_t intra_process_subscription_id = ipm->template add_subscription< + ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_); this->setup_intra_process(intra_process_subscription_id, ipm); } @@ -201,11 +200,11 @@ class Subscription : public SubscriptionBase this->subscription_topic_statistics_ = std::move(subscription_topic_statistics); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), static_cast(this)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), static_cast(&any_callback_)); @@ -316,7 +315,7 @@ class Subscription : public SubscriptionBase if (subscription_topic_statistics_) { const auto nanos = std::chrono::time_point_cast(now); const auto time = rclcpp::Time(nanos.time_since_epoch().count()); - subscription_topic_statistics_->handle_message(*typed_message, time); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); } } @@ -325,8 +324,20 @@ class Subscription : public SubscriptionBase const std::shared_ptr & serialized_message, const rclcpp::MessageInfo & message_info) override { - // TODO(wjwwood): enable topic statistics for serialized messages + std::chrono::time_point 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(); + } + any_callback_.dispatch(serialized_message, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(now); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); + } } void @@ -357,7 +368,7 @@ class Subscription : public SubscriptionBase if (subscription_topic_statistics_) { const auto nanos = std::chrono::time_point_cast(now); const auto time = rclcpp::Time(nanos.time_since_epoch().count()); - subscription_topic_statistics_->handle_message(*typed_message, time); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); } } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f4957824a5..615f3852b6 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -260,13 +260,13 @@ class SubscriptionBase : public std::enable_shared_from_this bool is_serialized() const; - /// Return the type of the subscription. + /// Return the delivered message kind. /** * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered. */ RCLCPP_PUBLIC DeliveredMessageKind - get_subscription_type() const; + get_delivered_message_kind() const; /// Get matching publisher count. /** \return The number of publishers on this topic. */ @@ -645,6 +645,14 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::node_interfaces::NodeBaseInterface * const node_base_; std::shared_ptr node_handle_; + + 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 on_new_message_callback_{nullptr}; + // Declare subscription_handle_ after callback std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; rclcpp::Logger node_logger_; @@ -663,15 +671,12 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_DISABLE_COPY(SubscriptionBase) rosidl_message_type_support_t type_support_; - DeliveredMessageKind delivered_message_type_; + DeliveredMessageKind delivered_message_kind_; std::atomic subscription_in_use_by_wait_set_{false}; std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; std::unordered_map> qos_events_in_use_by_wait_set_; - - std::recursive_mutex callback_mutex_; - std::function on_new_message_callback_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a1727eab5a..0e9d9fefe5 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -75,15 +75,14 @@ template< typename CallbackT, typename AllocatorT, typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType, - typename ROSMessageType = typename SubscriptionT::ROSMessageType + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType > SubscriptionFactory create_subscription_factory( CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, - std::shared_ptr> + std::shared_ptr subscription_topic_stats = nullptr ) { diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 43b9ec034c..71f90fe15d 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -77,6 +77,10 @@ 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; diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 15533f39ef..a931f3ac9c 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -57,6 +57,10 @@ class Time 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 @@ -84,6 +88,7 @@ class Time operator builtin_interfaces::msg::Time() const; /** + * Copy assignment operator * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC @@ -100,6 +105,13 @@ class Time 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 */ @@ -189,7 +201,7 @@ class Time */ RCLCPP_PUBLIC static Time - max(); + max(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); // NOLINT /// Get the seconds since epoch /** @@ -222,6 +234,15 @@ 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_ diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 91b1705985..6060d8bd78 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -53,12 +53,17 @@ class TimerBase * \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); + rclcpp::Context::SharedPtr context, + bool autostart = true); /// TimerBase destructor RCLCPP_PUBLIC @@ -216,21 +221,22 @@ class GenericTimer : public TimerBase * \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 + rclcpp::Context::SharedPtr context, bool autostart = true ) - : TimerBase(clock, period, context), callback_(std::forward(callback)) + : TimerBase(clock, period, context, autostart), callback_(std::forward(callback)) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_timer_callback_added, static_cast(get_timer_handle().get()), reinterpret_cast(&callback_)); #ifndef TRACETOOLS_DISABLED - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(callback_); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, reinterpret_cast(&callback_), symbol); @@ -269,9 +275,9 @@ class GenericTimer : public TimerBase void execute_callback() override { - TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); + TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); execute_callback_delegate<>(); - TRACEPOINT(callback_end, reinterpret_cast(&callback_)); + TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } // void specialization @@ -330,13 +336,15 @@ class WallTimer : public GenericTimer * \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) + rclcpp::Context::SharedPtr context, + bool autostart = true) : GenericTimer( - std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context) + std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context, autostart) {} protected: diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 4b9221406f..781e2c86fc 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -48,21 +48,12 @@ 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 + */ class SubscriptionTopicStatistics { - 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>; + using TopicStatsCollector = libstatistics_collector::TopicStatisticsCollector; + using ReceivedMessageAge = libstatistics_collector::ReceivedMessageAgeCollector; + using ReceivedMessagePeriod = libstatistics_collector::ReceivedMessagePeriodCollector; public: /// Construct a SubscriptionTopicStatistics object. @@ -101,16 +92,16 @@ class SubscriptionTopicStatistics /** * This method acquires a lock to prevent race conditions to collectors list. * - * \param received_message the message received by the subscription + * \param message_info the message info corresponding to the received message * \param now_nanoseconds current time in nanoseconds */ virtual void handle_message( - const CallbackMessageT & received_message, + const rmw_message_info_t & message_info, const rclcpp::Time now_nanoseconds) const { std::lock_guard lock(mutex_); for (const auto & collector : subscriber_statistics_collectors_) { - collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds()); + collector->OnMessageReceived(message_info, now_nanoseconds.nanoseconds()); } } diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp index 2fad84cf3b..c93b318440 100644 --- a/rclcpp/include/rclcpp/typesupport_helpers.hpp +++ b/rclcpp/include/rclcpp/typesupport_helpers.hpp @@ -22,6 +22,7 @@ #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" @@ -40,11 +41,15 @@ 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( @@ -52,6 +57,40 @@ 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_ diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index e879043d04..74d3d2183c 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -17,13 +17,21 @@ #include #include +#include +#include #include +#include #include "rcl/wait.h" #include "rclcpp/macros.hpp" #include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" + namespace rclcpp { @@ -134,6 +142,151 @@ class WaitResult final } } + /// Get the next ready timer and its index in the wait result, but do not clear it. + /** + * The returned timer is not cleared automatically, as it the case with the + * other next_ready_*()-like functions. + * Instead, this function returns the timer and the index that identifies it + * in the wait result, so that it can be cleared (marked as taken or used) + * in a separate step with clear_timer_with_index(). + * This is necessary in some multi-threaded executor implementations. + * + * If the timer is not cleared using the index, subsequent calls to this + * function will return the same timer. + * + * If there is no ready timer, then nullptr will be returned and the index + * will be invalid and should not be used. + * + * \param[in] start_index index at which to start searching for the next ready + * timer in the wait result. If the start_index is out of bounds for the + * list of timers in the wait result, then {nullptr, start_index} will be + * returned. Defaults to 0. + * \return next ready timer pointer and its index in the wait result, or + * {nullptr, start_index} if none was found. + */ + std::pair, size_t> + peek_next_ready_timer(size_t start_index = 0) + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + size_t ii = start_index; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (; ii < wait_set.size_of_timers(); ++ii) { + if (rcl_wait_set.timers[ii] != nullptr) { + ret = wait_set.timers(ii); + break; + } + } + } + return {ret, ii}; + } + + /// Clear the timer at the given index. + /** + * Clearing a timer from the wait result prevents it from being returned by + * the peek_next_ready_timer() on subsequent calls. + * + * The index should come from the peek_next_ready_timer() function, and + * should only be used with this function if the timer pointer was valid. + * + * \throws std::out_of_range if the given index is out of range + */ + void + clear_timer_with_index(size_t index) + { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + if (index >= wait_set.size_of_timers()) { + throw std::out_of_range("given timer index is out of range"); + } + rcl_wait_set.timers[index] = nullptr; + } + + /// Get the next ready subscription, clearing it from the wait result. + std::shared_ptr + next_ready_subscription() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_subscriptions(); ++ii) { + if (rcl_wait_set.subscriptions[ii] != nullptr) { + ret = wait_set.subscriptions(ii); + rcl_wait_set.subscriptions[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready service, clearing it from the wait result. + std::shared_ptr + next_ready_service() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_services(); ++ii) { + if (rcl_wait_set.services[ii] != nullptr) { + ret = wait_set.services(ii); + rcl_wait_set.services[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready client, clearing it from the wait result. + std::shared_ptr + next_ready_client() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_clients(); ++ii) { + if (rcl_wait_set.clients[ii] != nullptr) { + ret = wait_set.clients(ii); + rcl_wait_set.clients[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready waitable, clearing it from the wait result. + std::shared_ptr + next_ready_waitable() + { + check_wait_result_dirty(); + auto waitable = std::shared_ptr{nullptr}; + auto data = std::shared_ptr{nullptr}; + + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto rcl_wait_set = wait_set.get_rcl_wait_set(); + while (next_waitable_index_ < wait_set.size_of_waitables()) { + auto cur_waitable = wait_set.waitables(next_waitable_index_++); + if (cur_waitable != nullptr && cur_waitable->is_ready(rcl_wait_set)) { + waitable = cur_waitable; + break; + } + } + } + + return waitable; + } + private: RCLCPP_DISABLE_COPY(WaitResult) @@ -151,12 +304,25 @@ class WaitResult final // Should be enforced by the static factory methods on this class. assert(WaitResultKind::Ready == wait_result_kind); // Secure thread-safety (if provided) and shared ownership (if needed). - wait_set_pointer_->wait_result_acquire(); + this->get_wait_set().wait_result_acquire(); } - const WaitResultKind wait_result_kind_; + /// Check if the wait result is invalid because the wait set was modified. + void + check_wait_result_dirty() + { + // In the case that the wait set was modified while the result was out, + // we must mark the wait result as no longer valid + if (wait_set_pointer_ && this->get_wait_set().wait_result_dirty_) { + this->wait_result_kind_ = WaitResultKind::Invalid; + } + } + + WaitResultKind wait_result_kind_; WaitSetT * wait_set_pointer_ = nullptr; + + size_t next_waitable_index_ = 0; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp index 3ce65bf4f3..7980d1d127 100644 --- a/rclcpp/include/rclcpp/wait_result_kind.hpp +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -26,6 +26,7 @@ enum RCLCPP_PUBLIC WaitResultKind Ready, //get_subscription_handle().get(), + subscription_entry.subscription->get_subscription_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } @@ -271,8 +269,7 @@ class StoragePolicyCommon [this](const auto & inner_guard_conditions) { for (const auto & guard_condition : inner_guard_conditions) { - auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition); - if (nullptr == guard_condition_ptr_pair.second) { + if (!guard_condition) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -285,10 +282,10 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_guard_condition( &rcl_wait_set_, - &guard_condition_ptr_pair.second->get_rcl_guard_condition(), + &guard_condition->get_rcl_guard_condition(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } }; @@ -301,8 +298,7 @@ class StoragePolicyCommon // Add timers. for (const auto & timer : timers) { - auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer); - if (nullptr == timer_ptr_pair.second) { + if (!timer) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -315,17 +311,16 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_timer( &rcl_wait_set_, - timer_ptr_pair.second->get_timer_handle().get(), + timer->get_timer_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } // Add clients. for (const auto & client : clients) { - auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client); - if (nullptr == client_ptr_pair.second) { + if (!client) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -338,7 +333,7 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_client( &rcl_wait_set_, - client_ptr_pair.second->get_client_handle().get(), + client->get_client_handle().get(), nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -347,8 +342,7 @@ class StoragePolicyCommon // Add services. for (const auto & service : services) { - auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service); - if (nullptr == service_ptr_pair.second) { + if (!service) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -361,17 +355,16 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_service( &rcl_wait_set_, - service_ptr_pair.second->get_service_handle().get(), + service->get_service_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } // Add waitables. for (auto & waitable_entry : waitables) { - auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); - if (nullptr == waitable_ptr_pair.second) { + if (!waitable_entry.waitable) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -382,8 +375,7 @@ class StoragePolicyCommon needs_pruning_ = true; continue; } - rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - waitable.add_to_wait_set(&rcl_wait_set_); + waitable_entry.waitable->add_to_wait_set(rcl_wait_set_); } } @@ -405,6 +397,32 @@ class StoragePolicyCommon needs_resize_ = true; } + size_t size_of_subscriptions() const {return 0;} + size_t size_of_timers() const {return 0;} + size_t size_of_clients() const {return 0;} + size_t size_of_services() const {return 0;} + size_t size_of_waitables() const {return 0;} + + template + typename SubscriptionsIterable::value_type + subscriptions(size_t) const {return nullptr;} + + template + typename TimersIterable::value_type + timers(size_t) const {return nullptr;} + + template + typename ClientsIterable::value_type + clients(size_t) const {return nullptr;} + + template + typename ServicesIterable::value_type + services(size_t) const {return nullptr;} + + template + typename WaitablesIterable::value_type + waitables(size_t) const {return nullptr;} + rcl_wait_set_t rcl_wait_set_; rclcpp::Context::SharedPtr context_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 4cec85f39a..8f97596218 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -204,15 +204,19 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) { + this->storage_acquire_ownerships(); + this->storage_rebuild_rcl_wait_set_with_sets( - subscriptions_, - guard_conditions_, + shared_subscriptions_, + shared_guard_conditions_, extra_guard_conditions, - timers_, - clients_, - services_, - waitables_ + shared_timers_, + shared_clients_, + shared_services_, + shared_waitables_ ); + + this->storage_release_ownerships(); } template @@ -382,6 +386,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo return weak_ptr.expired(); }; // remove guard conditions which have been deleted + subscriptions_.erase( + std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end()); guard_conditions_.erase( std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p), guard_conditions_.end()); @@ -407,6 +413,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo } }; // Lock all the weak pointers and hold them until released. + lock_all(subscriptions_, shared_subscriptions_); lock_all(guard_conditions_, shared_guard_conditions_); lock_all(timers_, shared_timers_); lock_all(clients_, shared_clients_); @@ -438,6 +445,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo shared_ptr.reset(); } }; + reset_all(shared_subscriptions_); reset_all(shared_guard_conditions_); reset_all(shared_timers_); reset_all(shared_clients_); @@ -445,6 +453,61 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo reset_all(shared_waitables_); } + size_t size_of_subscriptions() const + { + return shared_subscriptions_.size(); + } + + size_t size_of_timers() const + { + return shared_timers_.size(); + } + + size_t size_of_clients() const + { + return shared_clients_.size(); + } + + size_t size_of_services() const + { + return shared_services_.size(); + } + + size_t size_of_waitables() const + { + return shared_waitables_.size(); + } + + std::shared_ptr + subscriptions(size_t ii) const + { + return shared_subscriptions_[ii].subscription; + } + + std::shared_ptr + timers(size_t ii) const + { + return shared_timers_[ii]; + } + + std::shared_ptr + clients(size_t ii) const + { + return shared_clients_[ii]; + } + + std::shared_ptr + services(size_t ii) const + { + return shared_services_[ii]; + } + + std::shared_ptr + waitables(size_t ii) const + { + return shared_waitables_[ii].waitable; + } + size_t ownership_reference_counter_ = 0; SequenceOfWeakSubscriptions subscriptions_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index be2e569c40..4afc2a1b27 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -290,7 +290,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon return create_wait_result(WaitResultKind::Empty); } else { // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed"); } } while (should_loop()); diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 434947c19f..7f5cad74ad 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -188,6 +188,61 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // Explicitly do nothing. } + size_t size_of_subscriptions() const + { + return subscriptions_.size(); + } + + size_t size_of_timers() const + { + return timers_.size(); + } + + size_t size_of_clients() const + { + return clients_.size(); + } + + size_t size_of_services() const + { + return services_.size(); + } + + size_t size_of_waitables() const + { + return waitables_.size(); + } + + typename ArrayOfSubscriptions::value_type + subscriptions(size_t ii) const + { + return subscriptions_[ii]; + } + + typename ArrayOfTimers::value_type + timers(size_t ii) const + { + return timers_[ii]; + } + + typename ArrayOfClients::value_type + clients(size_t ii) const + { + return clients_[ii]; + } + + typename ArrayOfServices::value_type + services(size_t ii) const + { + return services_[ii]; + } + + typename ArrayOfWaitables::value_type + waitables(size_t ii) const + { + return waitables_[ii]; + } + const ArrayOfSubscriptions subscriptions_; const ArrayOfGuardConditions guard_conditions_; const ArrayOfTimers timers_; diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index b3f41f8ed5..ce69da6bf2 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -153,6 +153,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("subscription already associated with a wait set"); } this->storage_add_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } if (mask.include_events) { for (auto key_event_pair : inner_subscription->get_event_handlers()) { @@ -164,6 +165,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("subscription event already associated with a wait set"); } this->storage_add_waitable(std::move(event), std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } if (mask.include_intra_process_waitable) { @@ -180,6 +182,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_add_waitable( std::move(inner_subscription->get_intra_process_waitable()), std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } }); @@ -224,6 +227,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false); this->storage_remove_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } if (mask.include_events) { for (auto key_event_pair : inner_subscription->get_event_handlers()) { @@ -231,6 +235,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); this->storage_remove_waitable(std::move(event)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } if (mask.include_intra_process_waitable) { @@ -239,6 +244,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // This is the case when intra process is enabled for the subscription. inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); this->storage_remove_waitable(std::move(local_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } }); @@ -289,6 +295,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the guard condition has already been added. this->storage_add_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -326,6 +333,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the guard condition is not in the wait set. this->storage_remove_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -357,6 +365,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the timer has already been added. this->storage_add_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -384,6 +393,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the timer is not in the wait set. this->storage_remove_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -415,6 +425,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the client has already been added. this->storage_add_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -442,6 +453,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the client is not in the wait set. this->storage_remove_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -473,6 +485,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the service has already been added. this->storage_add_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -500,6 +513,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the service is not in the wait set. this->storage_remove_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -551,6 +565,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the waitable has already been added. this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -578,6 +593,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the waitable is not in the wait set. this->storage_remove_waitable(std::move(inner_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -715,6 +731,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("wait_result_acquire() called while already holding"); } wait_result_holding_ = true; + wait_result_dirty_ = false; // this method comes from the SynchronizationPolicy this->sync_wait_result_acquire(); // this method comes from the StoragePolicy @@ -734,6 +751,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("wait_result_release() called while not holding"); } wait_result_holding_ = false; + wait_result_dirty_ = false; // this method comes from the StoragePolicy this->storage_release_ownerships(); // this method comes from the SynchronizationPolicy @@ -741,6 +759,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli } bool wait_result_holding_ = false; + bool wait_result_dirty_ = false; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 1a0d8b61f1..e834765a08 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -101,6 +101,23 @@ class Waitable size_t get_number_of_ready_guard_conditions(); +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa add_to_wait_set(rcl_wait_set_t &) + */ + [[deprecated("Use add_to_wait_set(rcl_wait_set_t & wait_set) signature")]] + RCLCPP_PUBLIC + virtual + void + add_to_wait_set(rcl_wait_set_t * wait_set); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. @@ -109,7 +126,24 @@ class Waitable RCLCPP_PUBLIC virtual void - add_to_wait_set(rcl_wait_set_t * wait_set) = 0; + add_to_wait_set(rcl_wait_set_t & wait_set); + +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa is_ready(const rcl_wait_set_t &) + */ + [[deprecated("Use is_ready(const rcl_wait_set_t & wait_set) signature")]] + RCLCPP_PUBLIC + virtual + bool + is_ready(rcl_wait_set_t * wait_set); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif /// Check if the Waitable is ready. /** @@ -124,7 +158,7 @@ class Waitable RCLCPP_PUBLIC virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; + is_ready(const rcl_wait_set_t & wait_set); /// Take the data so that it can be consumed with `execute`. /** @@ -178,6 +212,23 @@ class Waitable std::shared_ptr take_data_by_entity_id(size_t id); +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa execute(const std::shared_ptr &) + */ + [[deprecated("Use execute(const std::shared_ptr & data) signature")]] + RCLCPP_PUBLIC + virtual + void + execute(std::shared_ptr & data); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + /// Execute data that is passed in. /** * Before calling this method, the Waitable should be added to a wait set, @@ -203,7 +254,7 @@ class Waitable RCLCPP_PUBLIC virtual void - execute(std::shared_ptr & data) = 0; + execute(const std::shared_ptr & data); /// Exchange the "in use by wait set" state for this timer. /** diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 1823f4d463..103b1c1c33 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 21.1.0 + 28.0.0 The ROS client library in C++. Ivan Paunovic @@ -22,6 +22,7 @@ builtin_interfaces rcl_interfaces rosgraph_msgs + rosidl_runtime_c rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp @@ -29,12 +30,14 @@ builtin_interfaces rcl_interfaces rosgraph_msgs + rosidl_runtime_c rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp libstatistics_collector rcl + rcl_logging_interface rcl_yaml_param_parser rcpputils rcutils diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 769deacb11..2449cbe1f7 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -21,6 +21,7 @@ AnyExecutable::AnyExecutable() timer(nullptr), service(nullptr), client(nullptr), + waitable(nullptr), callback_group(nullptr), node_base(nullptr) {} diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 77f6c87bd9..bcacaabebe 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -31,12 +31,12 @@ using rclcpp::CallbackGroupType; CallbackGroup::CallbackGroup( CallbackGroupType group_type, - std::function get_context, + 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), - get_context_(get_context) + context_(context) {} CallbackGroup::~CallbackGroup() @@ -66,6 +66,7 @@ CallbackGroup::size() const timer_ptrs_.size() + waitable_ptrs_.size(); } + void CallbackGroup::collect_all_ptrs( std::function sub_func, std::function service_func, @@ -123,40 +124,12 @@ CallbackGroup::automatically_add_to_executor_with_node() const return automatically_add_to_executor_with_node_; } -// \TODO(mjcarroll) Deprecated, remove on tock -rclcpp::GuardCondition::SharedPtr -CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) -{ - std::lock_guard lock(notify_guard_condition_mutex_); - if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { - if (associated_with_executor_) { - trigger_notify_guard_condition(); - } - notify_guard_condition_ = nullptr; - } - - if (!notify_guard_condition_) { - notify_guard_condition_ = std::make_shared(context_ptr); - } - - return notify_guard_condition_; -} - rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition() { std::lock_guard lock(notify_guard_condition_mutex_); - if (!this->get_context_) { - throw std::runtime_error("Callback group was created without context and not passed context"); - } - auto context_ptr = this->get_context_(); + rclcpp::Context::SharedPtr context_ptr = context_.lock(); if (context_ptr && context_ptr->is_valid()) { - if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { - if (associated_with_executor_) { - trigger_notify_guard_condition(); - } - notify_guard_condition_ = nullptr; - } if (!notify_guard_condition_) { notify_guard_condition_ = std::make_shared(context_ptr); } diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index e33db71ce2..8388ee1888 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -125,7 +125,6 @@ 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(); @@ -138,6 +137,7 @@ 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() diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 35a11730ab..bbc1d29d0f 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -462,7 +462,7 @@ template std::vector Context::get_shutdown_callback() const { - const auto get_callback_vector = [this](auto & mutex, auto & callback_set) { + const auto get_callback_vector = [](auto & mutex, auto & callback_set) { const std::lock_guard lock(mutex); std::vector callbacks; for (auto & callback : callback_set) { @@ -496,7 +496,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) std::unique_lock lock(interrupt_mutex_); auto start = std::chrono::steady_clock::now(); // this will release the lock while waiting - interrupt_condition_variable_.wait_for(lock, nanoseconds); + interrupt_condition_variable_.wait_for(lock, time_left); time_left -= std::chrono::steady_clock::now() - start; } } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid()); diff --git a/rclcpp/src/rclcpp/create_generic_client.cpp b/rclcpp/src/rclcpp/create_generic_client.cpp new file mode 100644 index 0000000000..4b3b7ddc35 --- /dev/null +++ b/rclcpp/src/rclcpp/create_generic_client.cpp @@ -0,0 +1,44 @@ +// 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_base, + std::shared_ptr node_graph, + std::shared_ptr 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(cli); + node_services->add_client(cli_base_ptr, group); + return cli; +} +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp b/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp new file mode 100644 index 0000000000..1ca9892ac4 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp @@ -0,0 +1,37 @@ +// 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 + +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 diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2eac7f6b58..7cb678456e 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -28,6 +28,8 @@ #include "rcutils/logging_macros.h" +#include "rclcpp/utilities.hpp" + namespace rclcpp { @@ -316,4 +318,50 @@ 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(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(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 diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp index 40ae6c030d..6232e5b0d9 100644 --- a/rclcpp/src/rclcpp/event_handler.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -39,13 +39,6 @@ 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", @@ -63,9 +56,9 @@ EventHandlerBase::get_number_of_ready_events() /// Add the Waitable to a wait set. void -EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +EventHandlerBase::add_to_wait_set(rcl_wait_set_t & wait_set) { - rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_); + rcl_ret_t ret = rcl_wait_set_add_event(&wait_set, &event_handle_, &wait_set_event_index_); if (RCL_RET_OK != ret) { exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); } @@ -73,9 +66,9 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) /// Check if the Waitable is ready. bool -EventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +EventHandlerBase::is_ready(const rcl_wait_set_t & wait_set) { - return wait_set->events[wait_set_event_index_] == &event_handle_; + return wait_set.events[wait_set_event_index_] == &event_handle_; } void diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a7a2b8278b..a54d71e21b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include #include #include @@ -22,13 +24,14 @@ #include "rcl/allocator.h" #include "rcl/error_handling.h" +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/dynamic_typesupport/dynamic_message.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/guard_condition.hpp" -#include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" @@ -38,21 +41,29 @@ using namespace std::chrono_literals; -using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::Executor; +/// Mask to indicate to the waitset to only add the subscription. +/// The events and intraprocess waitable are already added via the callback group. +static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false}; + class rclcpp::ExecutorImplementation {}; Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), - memory_strategy_(options.memory_strategy), + context_(options.context), + notify_waitable_(std::make_shared( + [this]() { + this->entities_need_rebuild_.store(true); + })), + entities_need_rebuild_(true), + collector_(notify_waitable_), + wait_set_({}, {}, {}, {}, {}, {}, options.context), + current_notify_waitable_(notify_waitable_), impl_(std::make_unique()) { - // Store the context for later use. - context_ = options.context; - shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -61,74 +72,46 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); - // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, - // and one for the executor's guard cond (interrupt_guard_condition_) - 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()); - rcl_allocator_t allocator = memory_strategy_->get_allocator(); + notify_waitable_->set_on_ready_callback( + [this](auto, auto) { + this->entities_need_rebuild_.store(true); + }); - rcl_ret_t ret = rcl_wait_set_init( - &wait_set_, - 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), - allocator); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to create wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); - } + notify_waitable_->add_guard_condition(interrupt_guard_condition_); + notify_waitable_->add_guard_condition(shutdown_guard_condition_); } Executor::~Executor() { - // Disassociate all callback groups. - for (auto & pair : weak_groups_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes. - std::for_each( - 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(); - has_executor.store(false); - } + std::lock_guard guard(mutex_); + + notify_waitable_->remove_guard_condition(interrupt_guard_condition_); + notify_waitable_->remove_guard_condition(shutdown_guard_condition_); + current_collection_.timers.update( + {}, {}, + [this](auto timer) {wait_set_.remove_timer(timer);}); + + current_collection_.subscriptions.update( + {}, {}, + [this](auto subscription) { + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); }); - weak_nodes_.clear(); - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - weak_groups_to_nodes_.clear(); - for (const auto & pair : weak_groups_to_guard_conditions_) { - auto guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_guard_conditions_.clear(); - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_nodes_to_guard_conditions_.clear(); + current_collection_.clients.update( + {}, {}, + [this](auto client) {wait_set_.remove_client(client);}); - // Finalize the wait set. - if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - // 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()); + current_collection_.services.update( + {}, {}, + [this](auto service) {wait_set_.remove_service(service);}); + + current_collection_.guard_conditions.update( + {}, {}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); + + current_collection_.waitables.update( + {}, {}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -142,95 +125,39 @@ Executor::~Executor() std::vector Executor::get_all_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_all_callback_groups(); } std::vector Executor::get_manually_added_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_manually_added_callback_groups(); } std::vector Executor::get_automatically_added_callback_groups_from_nodes() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_automatically_added_callback_groups(); } void -Executor::add_callback_groups_from_nodes_associated_to_executor() -{ - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if ( - shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - this->add_callback_group_to_map( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_, - true); - } - }); - } - } -} - -void -Executor::add_callback_group_to_map( +Executor::add_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, bool notify) { - // If the callback_group already has an executor - 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."); - } - - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = - weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - // Also add to the map that contains all callback groups - weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); + (void) node_ptr; + this->collector_.add_callback_group(group_ptr); - if (node_ptr->get_context()->is_valid()) { - auto callback_group_guard_condition = group_ptr->get_notify_guard_condition(); - 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); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } if (notify) { - // Interrupt waiting to handle new node try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -241,91 +168,23 @@ Executor::add_callback_group_to_map( } } -void -Executor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) -{ - std::lock_guard guard{mutex_}; - this->add_callback_group_to_map( - group_ptr, - node_ptr, - weak_groups_associated_with_executor_to_nodes_, - notify); -} - void Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - // If the node already has an executor - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error( - std::string("Node '") + node_ptr->get_fully_qualified_name() + - "' has already been added to an executor."); - } - std::lock_guard guard{mutex_}; - node_ptr->for_each_callback_group( - [this, node_ptr, notify](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - if (!group_ptr->get_associated_with_executor_atomic().load() && - group_ptr->automatically_add_to_executor_with_node()) - { - this->add_callback_group_to_map( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); - } - }); + this->collector_.add_node(node_ptr); - const auto gc = node_ptr->get_shared_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = gc.get(); - // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(*gc); - weak_nodes_.push_back(node_ptr); -} - -void -Executor::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify) -{ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - weak_groups_to_nodes_.erase(group_ptr); - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && - !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) - { - auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); - if (iter != weak_groups_to_guard_conditions_.end()) { - memory_strategy_->remove_guard_condition(iter->second); - } - weak_groups_to_guard_conditions_.erase(weak_group_ptr); - - if (notify) { - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on callback group remove: ") + ex.what()); - } + + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node add: ") + ex.what()); } } } @@ -335,11 +194,21 @@ Executor::remove_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) { - std::lock_guard guard{mutex_}; - this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_, - notify); + this->collector_.remove_callback_group(group_ptr); + + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); + } + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group remove: ") + ex.what()); + } + } } void @@ -351,48 +220,22 @@ Executor::add_node(std::shared_ptr node_ptr, bool notify) void Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { - throw std::runtime_error("Node needs to be associated with an executor."); - } + this->collector_.remove_node(node_ptr); - std::lock_guard guard{mutex_}; - bool found_node = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - found_node = true; - node_it = weak_nodes_.erase(node_it); - } else { - ++node_it; - } - } - if (!found_node) { - throw std::runtime_error("Node needs to be associated with this executor."); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - for (auto it = weak_groups_to_nodes_associated_with_executor_.begin(); - it != weak_groups_to_nodes_associated_with_executor_.end(); ) - { - auto weak_node_ptr = it->second; - auto shared_node_ptr = weak_node_ptr.lock(); - auto group_ptr = it->first.lock(); - - // Increment iterator before removing in case it's invalidated - it++; - if (shared_node_ptr == node_ptr) { - remove_callback_group_from_map( - group_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node remove: ") + ex.what()); } } - - memory_strategy_->remove_guard_condition(node_ptr->get_shared_notify_guard_condition().get()); - weak_nodes_to_guard_conditions_.erase(node_ptr); - - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); } void @@ -431,6 +274,22 @@ 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 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) { @@ -459,20 +318,25 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) throw std::runtime_error("spin_some() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - bool work_available = false; + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { - AnyExecutable any_exec; - if (!work_available) { - wait_for_work(std::chrono::milliseconds::zero()); + if (!wait_result_.has_value()) { + wait_for_work(std::chrono::milliseconds(0)); } + + AnyExecutable any_exec; if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); - work_available = true; } else { - if (!work_available || !exhaustive) { - break; - } - work_available = false; + // If nothing is ready, reset the result to signal we are + // ready to wait again + wait_result_.reset(); + } + + if (!wait_result_.has_value() && !exhaustive) { + // In the case of spin some, then we can exit + // In the case of spin all, then we will allow ourselves to wait again. + break; } } } @@ -508,30 +372,21 @@ Executor::cancel() } } -void -Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) -{ - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor."); - } - std::lock_guard guard{mutex_}; - memory_strategy_ = memory_strategy; -} - void Executor::execute_any_executable(AnyExecutable & any_exec) { if (!spinning.load()) { return; } + if (any_exec.timer) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.timer->get_timer_handle().get())); execute_timer(any_exec.timer); } if (any_exec.subscription) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.subscription->get_subscription_handle().get())); execute_subscription(any_exec.subscription); @@ -543,18 +398,13 @@ Executor::execute_any_executable(AnyExecutable & any_exec) execute_client(any_exec.client); } if (any_exec.waitable) { - any_exec.waitable->execute(any_exec.data); + const std::shared_ptr & const_data = any_exec.data; + any_exec.waitable->execute(const_data); } + // Reset the callback_group, regardless of type - any_exec.callback_group->can_be_taken_from().store(true); - // Wake the wait, because it may need to be recalculated or work that - // was previously blocked is now available. - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition from execute_any_executable: ") + ex.what()); + if (any_exec.callback_group) { + any_exec.callback_group->can_be_taken_from().store(true); } } @@ -603,7 +453,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) rclcpp::MessageInfo message_info; message_info.get_rmw_message_info().from_intra_process = false; - switch (subscription->get_subscription_type()) { + switch (subscription->get_delivered_message_kind()) { // Deliver ROS message case rclcpp::DeliveredMessageKind::ROS_MESSAGE: { @@ -653,6 +503,11 @@ 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; @@ -689,7 +544,6 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) throw std::runtime_error("Delivered message kind is not supported"); } } - return; } void @@ -711,8 +565,7 @@ Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) } void -Executor::execute_client( - rclcpp::ClientBase::SharedPtr client) +Executor::execute_client(rclcpp::ClientBase::SharedPtr client) { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); @@ -724,227 +577,213 @@ Executor::execute_client( } void -Executor::wait_for_work(std::chrono::nanoseconds timeout) -{ - TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); - { - std::lock_guard guard(mutex_); - - // Check weak_nodes_ to find any callback group that is not owned - // by an executor and add it to the list of callbackgroups for - // collect entities. Also exchange to false so it is not - // allowed to add to another executor - add_callback_groups_from_nodes_associated_to_executor(); - - // Collect the subscriptions and timers to be waited on - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_); - - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (auto pair : weak_groups_to_nodes_) { - auto weak_group_ptr = pair.first; - auto weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr); - if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) { - auto guard_condition = node_guard_pair->second; - weak_nodes_to_guard_conditions_.erase(weak_node_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - if (weak_groups_to_nodes_associated_with_executor_.find(group_ptr) != - weak_groups_to_nodes_associated_with_executor_.end()) - { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - } - if (weak_groups_associated_with_executor_to_nodes_.find(group_ptr) != - weak_groups_associated_with_executor_to_nodes_.end()) - { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - } - auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr); - if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) { - auto guard_condition = callback_guard_pair->second; - weak_groups_to_guard_conditions_.erase(group_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_nodes_.erase(group_ptr); - }); - } +Executor::collect_entities() +{ + // Updating the entity collection and waitset expires any active result + this->wait_result_.reset(); + + // Get the current list of available waitables from the collector. + rclcpp::executors::ExecutorEntitiesCollection collection; + this->collector_.update_collections(); + auto callback_groups = this->collector_.get_all_callback_groups(); + rclcpp::executors::build_entities_collection(callback_groups, collection); + + // Make a copy of notify waitable so we can continue to mutate the original + // one outside of the execute loop. + // This prevents the collection of guard conditions in the waitable from changing + // while we are waiting on it. + if (notify_waitable_) { + current_notify_waitable_ = std::make_shared( + *notify_waitable_); + auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); + collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); + } + + // We must remove expired entities here, so that we don't continue to use older entities. + // See https://github.com/ros2/rclcpp/issues/2180 for more information. + current_collection_.remove_expired_entities(); + + // Update each of the groups of entities in the current collection, adding or removing + // from the wait set as necessary. + current_collection_.timers.update( + collection.timers, + [this](auto timer) {wait_set_.add_timer(timer);}, + [this](auto timer) {wait_set_.remove_timer(timer);}); + + current_collection_.subscriptions.update( + collection.subscriptions, + [this](auto subscription) { + wait_set_.add_subscription(subscription, kDefaultSubscriptionMask); + }, + [this](auto subscription) { + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); + }); - // clear wait set - rcl_ret_t ret = rcl_wait_set_clear(&wait_set_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Couldn't clear wait set"); - } + current_collection_.clients.update( + collection.clients, + [this](auto client) {wait_set_.add_client(client);}, + [this](auto client) {wait_set_.remove_client(client);}); - // The size of waitables are accounted for in size of the other entities - ret = rcl_wait_set_resize( - &wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Couldn't resize the wait set"); - } + current_collection_.services.update( + collection.services, + [this](auto service) {wait_set_.add_service(service);}, + [this](auto service) {wait_set_.remove_service(service);}); - if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - } + current_collection_.guard_conditions.update( + collection.guard_conditions, + [this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); - rcl_ret_t status = - rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(status, "rcl_wait() failed"); - } + current_collection_.waitables.update( + collection.waitables, + [this](auto waitable) {wait_set_.add_waitable(waitable);}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); - // check the null handles in the wait set and remove them from the handles in memory strategy - // for callback-based entities - std::lock_guard guard(mutex_); - memory_strategy_->remove_null_handles(&wait_set_); + // In the case that an entity already has an expired weak pointer + // before being removed from the waitset, additionally prune the waitset. + this->wait_set_.prune_deleted_entities(); + this->entities_need_rebuild_.store(false); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Executor::get_node_by_group( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group) +void +Executor::wait_for_work(std::chrono::nanoseconds timeout) { - if (!group) { - return nullptr; - } - rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group); - const auto finder = weak_groups_to_nodes.find(weak_group_ptr); - if (finder != weak_groups_to_nodes.end()) { - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr = finder->second.lock(); - return node_ptr; - } - return nullptr; -} + TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); -rclcpp::CallbackGroup::SharedPtr -Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) -{ - std::lock_guard guard{mutex_}; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (!group) { - continue; - } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; - } - } + // Clear any previous wait result + this->wait_result_.reset(); - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (!group) { - continue; - } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; + { + std::lock_guard guard(mutex_); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + this->collect_entities(); } } - return nullptr; + this->wait_result_.emplace(wait_set_.wait(timeout)); + if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + } } bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { - bool success = get_next_ready_executable_from_map(any_executable, weak_groups_to_nodes_); - return success; -} + TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready); -bool -Executor::get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) -{ - TRACEPOINT(rclcpp_executor_get_next_ready); - bool success = false; - std::lock_guard guard{mutex_}; - // Check the timers to see if there are any that are ready - memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes); - if (any_executable.timer) { - success = true; + bool valid_executable = false; + + if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) { + return false; } - if (!success) { - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_executable, weak_groups_to_nodes); - if (any_executable.subscription) { - success = true; + + if (!valid_executable) { + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result_->peek_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; + } + current_timer_index = timer_index; + auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); + if (entity_iter != current_collection_.timers.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + // At this point the timer is either ready for execution or was perhaps + // it was canceled, based on the result of call(), but either way it + // should not be checked again from peek_next_ready_timer(), so clear + // it from the wait result. + wait_result_->clear_timer_with_index(current_timer_index); + // Check that the timer should be called still, i.e. it wasn't canceled. + if (!timer->call()) { + continue; + } + any_executable.timer = timer; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - if (!success) { - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_executable, weak_groups_to_nodes); - if (any_executable.service) { - success = true; + + if (!valid_executable) { + while (auto subscription = wait_result_->next_ready_subscription()) { + auto entity_iter = current_collection_.subscriptions.find( + subscription->get_subscription_handle().get()); + if (entity_iter != current_collection_.subscriptions.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.subscription = subscription; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - if (!success) { - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_executable, weak_groups_to_nodes); - if (any_executable.client) { - success = true; + + if (!valid_executable) { + while (auto service = wait_result_->next_ready_service()) { + auto entity_iter = current_collection_.services.find(service->get_service_handle().get()); + if (entity_iter != current_collection_.services.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.service = service; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - if (!success) { - // Check the waitables to see if there are any that are ready - memory_strategy_->get_next_waitable(any_executable, weak_groups_to_nodes); - if (any_executable.waitable) { - any_executable.data = any_executable.waitable->take_data(); - success = true; + + if (!valid_executable) { + while (auto client = wait_result_->next_ready_client()) { + auto entity_iter = current_collection_.clients.find(client->get_client_handle().get()); + if (entity_iter != current_collection_.clients.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.client = client; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - // At this point any_executable should be valid with either a valid subscription - // or a valid timer, or it should be a null shared_ptr - if (success) { - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = any_executable.callback_group; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter == weak_groups_to_nodes.end()) { - success = false; + + if (!valid_executable) { + while (auto waitable = wait_result_->next_ready_waitable()) { + auto entity_iter = current_collection_.waitables.find(waitable.get()); + if (entity_iter != current_collection_.waitables.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.waitable = waitable; + any_executable.callback_group = callback_group; + any_executable.data = waitable->take_data(); + valid_executable = true; + break; + } } } - if (success) { - // If it is valid, check to see if the group is mutually exclusive or - // not, then mark it accordingly ..Check if the callback_group belongs to this executor - if (any_executable.callback_group && any_executable.callback_group->type() == \ - CallbackGroupType::MutuallyExclusive) - { - // It should not have been taken otherwise + if (any_executable.callback_group) { + if (any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) { assert(any_executable.callback_group->can_be_taken_from().load()); - // Set to false to indicate something is being run from this group - // This is reset to true either when the any_executable is executed or when the - // any_executable is destructued any_executable.callback_group->can_be_taken_from().store(false); } } - // If there is no ready executable, return false - return success; + + + return valid_executable; } bool @@ -967,22 +806,6 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos return success; } -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. -bool -Executor::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); -} - bool Executor::is_spinning() { diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 0a900c07da..3ea8d658ad 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -14,6 +14,21 @@ #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) { diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 88a878824a..765002b2ef 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -39,6 +39,30 @@ void ExecutorEntitiesCollection::clear() waitables.clear(); } +size_t ExecutorEntitiesCollection::remove_expired_entities() +{ + auto remove_entities = [](auto & collection) -> size_t { + size_t removed = 0; + for (auto it = collection.begin(); it != collection.end(); ) { + if (it->second.entity.expired()) { + ++removed; + it = collection.erase(it); + } else { + ++it; + } + } + return removed; + }; + + return + remove_entities(subscriptions) + + remove_entities(timers) + + remove_entities(guard_conditions) + + remove_entities(clients) + + remove_entities(services) + + remove_entities(waitables); +} + void build_entities_collection( const std::vector & callback_groups, @@ -203,12 +227,12 @@ ready_executables( } } - for (auto & [handle, entry] : collection.waitables) { + for (const auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); if (!waitable) { continue; } - if (!waitable->is_ready(&rcl_wait_set)) { + if (!waitable->is_ready(rcl_wait_set)) { continue; } auto group_info = group_cache(entry.callback_group); @@ -218,13 +242,10 @@ ready_executables( 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 diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 84ada64925..702716a758 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -61,6 +61,12 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() if (group_ptr) { group_ptr->get_associated_with_executor_atomic().store(false); } + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + 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); + } } pending_manually_added_groups_.clear(); pending_manually_removed_groups_.clear(); @@ -105,7 +111,8 @@ void ExecutorEntitiesCollector::remove_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (!has_executor.exchange(false)) { throw std::runtime_error( std::string("Node '") + node_ptr->get_fully_qualified_name() + "' needs to be associated with an executor."); @@ -143,6 +150,11 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g } this->pending_manually_added_groups_.insert(group_ptr); + + // Store callback group notify 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 @@ -161,7 +173,6 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt 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 lock(mutex_); bool associated = manually_added_groups_.count(group_ptr) != 0; @@ -314,7 +325,11 @@ ExecutorEntitiesCollector::process_queues() if (node_it != weak_nodes_.end()) { remove_weak_node(node_it); } else { - throw std::runtime_error("Node needs to be associated with this executor."); + // The node may have been destroyed and removed from the colletion before + // we processed the queues. Don't throw if the pointer is already expired. + if (!weak_node_ptr.expired()) { + throw std::runtime_error("Node needs to be associated with this executor."); + } } auto node_ptr = weak_node_ptr.lock(); @@ -337,6 +352,13 @@ ExecutorEntitiesCollector::process_queues() auto group_ptr = weak_group_ptr.lock(); if (group_ptr) { this->add_callback_group_to_collection(group_ptr, manually_added_groups_); + } else { + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + 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); + } } } pending_manually_added_groups_.clear(); diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 15a31cd60d..a95bc3797c 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -27,15 +27,17 @@ ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_exec { } -ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other) -: ExecutorNotifyWaitable(other.execute_callback_) +ExecutorNotifyWaitable::ExecutorNotifyWaitable(ExecutorNotifyWaitable & other) { + std::lock_guard lock(other.guard_condition_mutex_); + this->execute_callback_ = other.execute_callback_; this->notify_guard_conditions_ = other.notify_guard_conditions_; } -ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other) +ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(ExecutorNotifyWaitable & other) { if (this != &other) { + std::lock_guard lock(other.guard_condition_mutex_); this->execute_callback_ = other.execute_callback_; this->notify_guard_conditions_ = other.notify_guard_conditions_; } @@ -43,43 +45,40 @@ ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyW } void -ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) +ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) { std::lock_guard 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(); + if (!guard_condition) {continue;} - rcl_ret_t ret = rcl_wait_set_add_guard_condition( - wait_set, - rcl_guard_condition, NULL); + rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition(); + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, cond, NULL); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to add guard condition to wait set"); - } + 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) +ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t & wait_set) { std::lock_guard 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]; + for (size_t ii = 0; ii < wait_set.size_of_guard_conditions; ++ii) { + const auto * rcl_guard_condition = wait_set.guard_conditions[ii]; if (nullptr == rcl_guard_condition) { continue; } - for (auto weak_guard_condition : this->notify_guard_conditions_) { + for (const 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; + break; } } } @@ -87,7 +86,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) } void -ExecutorNotifyWaitable::execute(std::shared_ptr & data) +ExecutorNotifyWaitable::execute(const std::shared_ptr & data) { (void) data; this->execute_callback_(); diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 507d47f913..2d5c76e817 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -55,7 +55,7 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { @@ -99,6 +99,18 @@ MultiThreadedExecutor::run(size_t this_thread_number) execute_any_executable(any_exec); + if (any_exec.callback_group && + any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) + { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group change: ") + ex.what()); + } + } + // Clear the callback_group to prevent the AnyExecutable destructor from // resetting the callback group `can_be_taken_from` any_exec.callback_group.reset(); diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index e7f311c147..975733b497 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -31,6 +31,11 @@ SingleThreadedExecutor::spin() throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + // Clear any previous result and rebuild the waitset + this->wait_result_.reset(); + this->entities_need_rebuild_ = true; + while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::AnyExecutable any_executable; if (get_next_executable(any_executable)) { diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp deleted file mode 100644 index 6fd0b56a85..0000000000 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ /dev/null @@ -1,524 +0,0 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT 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/static_executor_entities_collector.hpp" - -#include -#include -#include -#include -#include -#include - -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/executors/static_single_threaded_executor.hpp" -#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" - -using rclcpp::executors::StaticExecutorEntitiesCollector; - -StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector() -{ - // Disassociate all callback groups and thus nodes. - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes - for (const auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - std::atomic_bool & has_executor = node->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - exec_list_.clear(); - weak_nodes_.clear(); - weak_nodes_to_guard_conditions_.clear(); -} - -void -StaticExecutorEntitiesCollector::init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) -{ - // Empty initialize executable list - exec_list_ = rclcpp::experimental::ExecutableList(); - // Get executor's wait_set_ pointer - p_wait_set_ = p_wait_set; - // Get executor's memory strategy ptr - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor waitable."); - } - memory_strategy_ = memory_strategy; - - // Get memory strategy and executable list. Prepare wait_set_ - std::shared_ptr shared_ptr; - execute(shared_ptr); - - // The entities collector is now initialized - initialized_ = true; -} - -void -StaticExecutorEntitiesCollector::fini() -{ - memory_strategy_->clear_handles(); - exec_list_.clear(); -} - -std::shared_ptr -StaticExecutorEntitiesCollector::take_data() -{ - return nullptr; -} - -void -StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) -{ - (void) data; - // Fill memory strategy with entities coming from weak_nodes_ - fill_memory_strategy(); - // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy) - fill_executable_list(); - // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize) - prepare_wait_set(); - // Add new nodes guard conditions to map - std::lock_guard 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(); - } - } - new_nodes_.clear(); -} - -void -StaticExecutorEntitiesCollector::fill_memory_strategy() -{ - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto & weak_group_ptr = pair.first; - auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - }); - } - has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto & weak_group_ptr = pair.first; - const auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - }); - } - - // Add the static executor waitable to the memory strategy - memory_strategy_->add_waitable_handle(this->shared_from_this()); -} - -void -StaticExecutorEntitiesCollector::fill_executable_list() -{ - exec_list_.clear(); - add_callback_groups_from_nodes_associated_to_executor(); - fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_); - fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_); - // Add the executor's waitable to the executable list - exec_list_.add_waitable(shared_from_this()); -} -void -StaticExecutorEntitiesCollector::fill_executable_list_from_map( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) -{ - for (const auto & pair : weak_groups_to_nodes) { - auto group = pair.first.lock(); - auto node = pair.second.lock(); - if (!node || !group || !group->can_be_taken_from().load()) { - continue; - } - group->find_timer_ptrs_if( - [this](const rclcpp::TimerBase::SharedPtr & timer) { - if (timer) { - exec_list_.add_timer(timer); - } - return false; - }); - group->find_subscription_ptrs_if( - [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { - if (subscription) { - exec_list_.add_subscription(subscription); - } - return false; - }); - group->find_service_ptrs_if( - [this](const rclcpp::ServiceBase::SharedPtr & service) { - if (service) { - exec_list_.add_service(service); - } - return false; - }); - group->find_client_ptrs_if( - [this](const rclcpp::ClientBase::SharedPtr & client) { - if (client) { - exec_list_.add_client(client); - } - return false; - }); - group->find_waitable_ptrs_if( - [this](const rclcpp::Waitable::SharedPtr & waitable) { - if (waitable) { - exec_list_.add_waitable(waitable); - } - return false; - }); - } -} - -void -StaticExecutorEntitiesCollector::prepare_wait_set() -{ - // clear wait set - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - // The size of waitables are accounted for in size of the other entities - rcl_ret_t ret = rcl_wait_set_resize( - p_wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str); - } -} - -void -StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout) -{ - // clear wait set (memset to '0' all wait_set_ entities - // but keeps the wait_set_ number of entities) - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - - rcl_ret_t status = - rcl_wait(p_wait_set_, std::chrono::duration_cast(timeout).count()); - - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(status, "rcl_wait() failed"); - } -} - -void -StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - // Add waitable guard conditions (one for each registered node) into the wait set. - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto & gc = pair.second; - detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc); - } -} - -size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() -{ - std::lock_guard guard{new_nodes_mutex_}; - return weak_nodes_to_guard_conditions_.size() + new_nodes_.size(); -} - -bool -StaticExecutorEntitiesCollector::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - bool is_new_node = false; - // If the node already has an executor - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error("Node has already been added to an executor."); - } - node_ptr->for_each_callback_group( - [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - if ( - !group_ptr->get_associated_with_executor_atomic().load() && - group_ptr->automatically_add_to_executor_with_node()) - { - is_new_node = (add_callback_group( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_) || - is_new_node); - } - }); - weak_nodes_.push_back(node_ptr); - return is_new_node; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - // If the callback_group already has an executor - 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."); - } - bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_); - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = weak_groups_to_nodes.insert( - std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - if (is_new_node) { - std::lock_guard guard{new_nodes_mutex_}; - new_nodes_.push_back(node_ptr); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr) -{ - return this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); - } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_)) - { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_.erase(node_weak_ptr); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - if (!node_ptr->get_associated_with_executor_atomic().load()) { - return false; - } - bool node_found = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - weak_nodes_.erase(node_it); - node_found = true; - break; - } - ++node_it; - } - if (!node_found) { - return false; - } - std::vector found_group_ptrs; - 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 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(); - if (shared_node_ptr == node_ptr) { - found_group_ptrs.push_back(group_ptr); - } - }); - std::for_each( - 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_); - }); - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - return true; -} - -bool -StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) -{ - // Check wait_set guard_conditions for added/removed entities to/from a node - for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) { - if (p_wait_set->guard_conditions[i] != NULL) { - auto found_guard_condition = std::find_if( - weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(), - [&](std::pair pair) -> bool { - const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition(); - return &rcl_gc == p_wait_set->guard_conditions[i]; - }); - if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) { - return true; - } - } - } - // None of the guard conditions triggered belong to a registered node - return false; -} - -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. -bool -StaticExecutorEntitiesCollector::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); -} - -void -StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor() -{ - for (const auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if (shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - add_callback_group( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_); - } - }); - } - } -} - -std::vector -StaticExecutorEntitiesCollector::get_all_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_manually_added_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 3c14b37b45..831076c61c 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -12,31 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/executors/static_single_threaded_executor.hpp" - -#include -#include -#include -#include - +#include "rclcpp/executors/executor_entities_collection.hpp" #include "rcpputils/scope_exit.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/any_executable.hpp" + using rclcpp::executors::StaticSingleThreadedExecutor; -using rclcpp::experimental::ExecutableList; -StaticSingleThreadedExecutor::StaticSingleThreadedExecutor( - const rclcpp::ExecutorOptions & options) +StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options) : rclcpp::Executor(options) { - entities_collector_ = std::make_shared(); } -StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() -{ - if (entities_collector_->is_init()) { - entities_collector_->fini(); - } -} +StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} void StaticSingleThreadedExecutor::spin() @@ -46,14 +35,11 @@ StaticSingleThreadedExecutor::spin() } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - // Set memory_strategy_ and exec_list_ based on weak_nodes_ - // Prepare wait_set_ based on memory_strategy_ - entities_collector_->init(&wait_set_, memory_strategy_); - + // This is essentially the contents of the rclcpp::Executor::wait_for_work method, + // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor + // behavior. while (rclcpp::ok(this->context_) && spinning.load()) { - // Refresh wait set and wait for work - entities_collector_->refresh_wait_set(); - execute_ready_executables(); + this->spin_once_impl(std::chrono::nanoseconds(-1)); } } @@ -64,7 +50,6 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) if (std::chrono::nanoseconds(0) == max_duration) { max_duration = std::chrono::nanoseconds::max(); } - return this->spin_some_impl(max_duration, false); } @@ -80,36 +65,32 @@ StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) void StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) { - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_); - } - auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { - if (std::chrono::nanoseconds(0) == max_duration) { - // told to spin forever if need be - return true; - } else if (std::chrono::steady_clock::now() - start < max_duration) { - // told to spin only for some maximum amount of time - return true; - } - // spun too long - return false; + const auto spin_forever = std::chrono::nanoseconds(0) == max_duration; + const auto cur_duration = std::chrono::steady_clock::now() - start; + return spin_forever || (cur_duration < max_duration); }; if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { // Get executables that are ready now - entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero()); - // Execute ready executables - bool work_available = execute_ready_executables(); - if (!work_available || !exhaustive) { - break; + std::lock_guard guard(mutex_); + + auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(0)); + if (wait_result.has_value()) { + // Execute ready executables + bool work_available = this->execute_ready_executables( + current_collection_, + wait_result.value(), + false); + if (!work_available || !exhaustive) { + break; + } } } } @@ -117,163 +98,95 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati void StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) { - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_); - } - if (rclcpp::ok(context_) && spinning.load()) { - // Wait until we have a ready entity or timeout expired - entities_collector_->refresh_wait_set(timeout); - // Execute ready executables - execute_ready_executables(true); + std::lock_guard guard(mutex_); + auto wait_result = this->collect_and_wait(timeout); + if (wait_result.has_value()) { + this->execute_ready_executables(current_collection_, wait_result.value(), true); + } } } -void -StaticSingleThreadedExecutor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) +std::optional> +StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout) { - 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(); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + this->collect_entities(); } -} - -void -StaticSingleThreadedExecutor::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - 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(); + auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + return {}; } + return wait_result; } -void -StaticSingleThreadedExecutor::add_node(std::shared_ptr node_ptr, bool notify) +// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor +// from the original implementation. +bool StaticSingleThreadedExecutor::execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once) { - this->add_node(node_ptr->get_node_base_interface(), notify); -} - -void -StaticSingleThreadedExecutor::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) -{ - 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(); + bool any_ready_executable = false; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return any_ready_executable; } -} -void -StaticSingleThreadedExecutor::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - bool node_removed = entities_collector_->remove_node(node_ptr); - if (!node_removed) { - throw std::runtime_error("Node needs to be associated with this executor."); - } - // If the node was matched and removed, interrupt waiting - if (notify) { - interrupt_guard_condition_->trigger(); + while (auto subscription = wait_result.next_ready_subscription()) { + auto entity_iter = collection.subscriptions.find(subscription->get_subscription_handle().get()); + if (entity_iter != collection.subscriptions.end()) { + execute_subscription(subscription); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } } -} - -std::vector -StaticSingleThreadedExecutor::get_all_callback_groups() -{ - return entities_collector_->get_all_callback_groups(); -} - -std::vector -StaticSingleThreadedExecutor::get_manually_added_callback_groups() -{ - return entities_collector_->get_manually_added_callback_groups(); -} -std::vector -StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes() -{ - return entities_collector_->get_automatically_added_callback_groups_from_nodes(); -} - -void -StaticSingleThreadedExecutor::remove_node(std::shared_ptr node_ptr, bool notify) -{ - this->remove_node(node_ptr->get_node_base_interface(), notify); -} - -bool -StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once) -{ - bool any_ready_executable = false; - - // Execute all the ready subscriptions - for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) { - if (i < entities_collector_->get_number_of_subscriptions()) { - if (wait_set_.subscriptions[i]) { - execute_subscription(entities_collector_->get_subscription(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result.peek_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; } - } - // Execute all the ready timers - for (size_t i = 0; i < wait_set_.size_of_timers; ++i) { - if (i < entities_collector_->get_number_of_timers()) { - if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) { - auto timer = entities_collector_->get_timer(i); - timer->call(); - execute_timer(std::move(timer)); - if (spin_once) { - return true; - } + current_timer_index = timer_index; + auto entity_iter = collection.timers.find(timer->get_timer_handle().get()); + if (entity_iter != collection.timers.end()) { + wait_result.clear_timer_with_index(current_timer_index); + if (timer->call()) { + execute_timer(timer); any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } } - // Execute all the ready services - for (size_t i = 0; i < wait_set_.size_of_services; ++i) { - if (i < entities_collector_->get_number_of_services()) { - if (wait_set_.services[i]) { - execute_service(entities_collector_->get_service(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + + while (auto client = wait_result.next_ready_client()) { + auto entity_iter = collection.clients.find(client->get_client_handle().get()); + if (entity_iter != collection.clients.end()) { + execute_client(client); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } - // Execute all the ready clients - for (size_t i = 0; i < wait_set_.size_of_clients; ++i) { - if (i < entities_collector_->get_number_of_clients()) { - if (wait_set_.clients[i]) { - execute_client(entities_collector_->get_client(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + + while (auto service = wait_result.next_ready_service()) { + auto entity_iter = collection.services.find(service->get_service_handle().get()); + if (entity_iter != collection.services.end()) { + execute_service(service); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } - // Execute all the ready waitables - for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) { - auto waitable = entities_collector_->get_waitable(i); - if (waitable->is_ready(&wait_set_)) { - auto data = waitable->take_data(); + + while (auto waitable = wait_result.next_ready_waitable()) { + auto entity_iter = collection.waitables.find(waitable.get()); + if (entity_iter != collection.waitables.end()) { + const auto data = waitable->take_data(); waitable->execute(data); - if (spin_once) { - return true; - } any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } return any_ready_executable; diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index 094ff21282..f7ba6da2df 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -50,6 +50,9 @@ EventsExecutor::EventsExecutor( timers_manager_ = std::make_shared(context_, timer_on_ready_cb); + this->current_entities_collection_ = + std::make_shared(); + notify_waitable_ = std::make_shared( [this]() { // This callback is invoked when: @@ -61,6 +64,10 @@ EventsExecutor::EventsExecutor( this->refresh_current_collection_from_callback_groups(); }); + // Make sure that the notify waitable is immediately added to the collection + // to avoid missing events + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); + notify_waitable_->add_guard_condition(interrupt_guard_condition_); notify_waitable_->add_guard_condition(shutdown_guard_condition_); @@ -87,9 +94,6 @@ EventsExecutor::EventsExecutor( this->entities_collector_ = std::make_shared(notify_waitable_); - - this->current_entities_collection_ = - std::make_shared(); } EventsExecutor::~EventsExecutor() @@ -202,11 +206,12 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout) timeout = std::chrono::nanoseconds::max(); } - // Select the smallest between input timeout and timer timeout + // Select the smallest between input timeout and timer timeout. + // Cancelled timers are not considered. bool is_timer_timeout = false; auto next_timer_timeout = timers_manager_->get_head_timeout(); - if (next_timer_timeout < timeout) { - timeout = next_timer_timeout; + if (next_timer_timeout.has_value() && next_timer_timeout.value() < timeout) { + timeout = next_timer_timeout.value(); is_timer_timeout = true; } @@ -269,10 +274,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) switch (event.type) { case ExecutorEventType::CLIENT_EVENT: { - auto client = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->clients); - + rclcpp::ClientBase::SharedPtr client; + { + std::lock_guard lock(collection_mutex_); + client = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->clients); + } if (client) { for (size_t i = 0; i < event.num_events; i++) { execute_client(client); @@ -283,9 +291,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SUBSCRIPTION_EVENT: { - auto subscription = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->subscriptions); + rclcpp::SubscriptionBase::SharedPtr subscription; + { + std::lock_guard lock(collection_mutex_); + subscription = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->subscriptions); + } if (subscription) { for (size_t i = 0; i < event.num_events; i++) { execute_subscription(subscription); @@ -295,10 +307,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SERVICE_EVENT: { - auto service = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->services); - + rclcpp::ServiceBase::SharedPtr service; + { + std::lock_guard lock(collection_mutex_); + service = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->services); + } if (service) { for (size_t i = 0; i < event.num_events; i++) { execute_service(service); @@ -315,12 +330,16 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::WAITABLE_EVENT: { - auto waitable = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->waitables); + rclcpp::Waitable::SharedPtr waitable; + { + std::lock_guard lock(collection_mutex_); + waitable = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->waitables); + } if (waitable) { for (size_t i = 0; i < event.num_events; i++) { - auto data = waitable->take_data_by_entity_id(event.waitable_data); + const auto data = waitable->take_data_by_entity_id(event.waitable_data); waitable->execute(data); } } @@ -382,6 +401,7 @@ EventsExecutor::get_automatically_added_callback_groups_from_nodes() void EventsExecutor::refresh_current_collection_from_callback_groups() { + // Build the new collection this->entities_collector_->update_collections(); auto callback_groups = this->entities_collector_->get_all_callback_groups(); rclcpp::executors::ExecutorEntitiesCollection new_collection; @@ -395,18 +415,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups() // retrieved in the "standard" way. // To do it, we need to add the notify waitable as an entry in both the new and // current collections such that it's neither added or removed. - rclcpp::CallbackGroup::WeakPtr weak_group_ptr; - new_collection.waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); + this->add_notify_waitable_to_collection(new_collection.waitables); - this->current_entities_collection_->waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); this->refresh_current_collection(new_collection); } @@ -415,6 +428,9 @@ void EventsExecutor::refresh_current_collection( const rclcpp::executors::ExecutorEntitiesCollection & new_collection) { + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); + current_entities_collection_->timers.update( new_collection.timers, [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, @@ -486,3 +502,16 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) }; return callback; } + +void +EventsExecutor::add_notify_waitable_to_collection( + rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection) +{ + // The notify waitable is not associated to any group, so use an invalid one + rclcpp::CallbackGroup::WeakPtr weak_group_ptr; + collection.insert( + { + this->notify_waitable_.get(), + {this->notify_waitable_, weak_group_ptr} + }); +} diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp index f4ecd16b76..39924afa56 100644 --- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -28,9 +28,9 @@ using rclcpp::experimental::TimersManager; TimersManager::TimersManager( std::shared_ptr context, std::function on_ready_callback) +: on_ready_callback_(on_ready_callback), + context_(context) { - context_ = context; - on_ready_callback_ = on_ready_callback; } TimersManager::~TimersManager() @@ -100,7 +100,7 @@ void TimersManager::stop() } } -std::chrono::nanoseconds TimersManager::get_head_timeout() +std::optional TimersManager::get_head_timeout() { // Do not allow to interfere with the thread running if (running_) { @@ -169,7 +169,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) } } -std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() +std::optional TimersManager::get_head_timeout_unsafe() { // If we don't have any weak pointer, then we just return maximum timeout if (weak_timers_heap_.empty()) { @@ -191,7 +191,9 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() } head_timer = locked_heap.front(); } - + if (head_timer->is_canceled()) { + return std::nullopt; + } return head_timer->time_until_trigger(); } @@ -242,17 +244,34 @@ void TimersManager::run_timers() // Lock mutex std::unique_lock lock(timers_mutex_); - std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(); + std::optional time_to_sleep = get_head_timeout_unsafe(); + + // If head timer was cancelled, try to reheap and get a new head. + // This avoids an edge condition where head timer is cancelled, but other + // valid timers remain in the heap. + if (!time_to_sleep.has_value()) { + // Re-heap to (possibly) move cancelled timer from head of heap. If + // entire heap is cancelled, this will still result in a nullopt. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.heapify(); + weak_timers_heap_.store(locked_heap); + time_to_sleep = get_head_timeout_unsafe(); + } - // No need to wait if a timer is already available - if (time_to_sleep > std::chrono::nanoseconds::zero()) { - if (time_to_sleep != std::chrono::nanoseconds::max()) { - // Wait until timeout or notification that timers have been updated - timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;}); - } else { - // Wait until notification that timers have been updated - timers_cv_.wait(lock, [this]() {return timers_updated_;}); - } + // If no timers, or all timers cancelled, wait for an update. + if (!time_to_sleep.has_value() || (time_to_sleep.value() == std::chrono::nanoseconds::max()) ) { + // Wait until notification that timers have been updated + timers_cv_.wait(lock, [this]() {return timers_updated_;}); + + // Re-heap in case ordering changed due to a cancelled timer + // re-activating. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.heapify(); + weak_timers_heap_.store(locked_heap); + } else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) { + // If time_to_sleep is zero, we immediately execute. Otherwise, wait + // until timeout or notification that timers have been updated + timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;}); } // Reset timers updated flag diff --git a/rclcpp/src/rclcpp/generic_client.cpp b/rclcpp/src/rclcpp/generic_client.cpp new file mode 100644 index 0000000000..fdcfc70aab --- /dev/null +++ b/rclcpp/src/rclcpp/generic_client.cpp @@ -0,0 +1,164 @@ +// 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 + +#include "rclcpp/generic_client.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +namespace rclcpp +{ +GenericClient::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) +: ClientBase(node_base, node_graph) +{ + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + auto service_ts_ = get_service_typesupport_handle( + service_type, "rosidl_typesupport_cpp", *ts_lib_); + + auto response_type_support_intro = get_message_typesupport_handle( + service_ts_->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + response_members_ = static_cast( + response_type_support_intro->data); + + rcl_ret_t ret = rcl_client_init( + this->get_client_handle().get(), + this->get_rcl_node_handle(), + service_ts_, + service_name.c_str(), + &client_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = this->get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create generic client"); + } +} + +std::shared_ptr +GenericClient::create_response() +{ + void * response = new uint8_t[response_members_->size_of_]; + response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + response, + [this](void * p) + { + response_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericClient::create_request_header() +{ + // TODO(wjwwood): This should probably use rmw_request_id's allocator. + // (since it is a C type) + return std::shared_ptr(new rmw_request_id_t); +} + +void +GenericClient::handle_response( + std::shared_ptr request_header, + std::shared_ptr response) +{ + auto optional_pending_request = + this->get_and_erase_pending_request(request_header->sequence_number); + if (!optional_pending_request) { + return; + } + auto & value = *optional_pending_request; + if (std::holds_alternative(value)) { + auto & promise = std::get(value); + promise.set_value(std::move(response)); + } +} + +size_t +GenericClient::prune_pending_requests() +{ + std::lock_guard guard(pending_requests_mutex_); + auto ret = pending_requests_.size(); + pending_requests_.clear(); + return ret; +} + +bool +GenericClient::remove_pending_request(int64_t request_id) +{ + std::lock_guard guard(pending_requests_mutex_); + return pending_requests_.erase(request_id) != 0u; +} + +std::optional +GenericClient::get_and_erase_pending_request(int64_t request_number) +{ + std::unique_lock lock(pending_requests_mutex_); + auto it = pending_requests_.find(request_number); + if (it == pending_requests_.end()) { + RCUTILS_LOG_DEBUG_NAMED( + "rclcpp", + "Received invalid sequence number. Ignoring..."); + return std::nullopt; + } + auto value = std::move(it->second.second); + pending_requests_.erase(request_number); + return value; +} + +GenericClient::FutureAndRequestId +GenericClient::async_send_request(const Request request) +{ + Promise promise; + auto future = promise.get_future(); + auto req_id = async_send_request_impl( + request, + std::move(promise)); + return FutureAndRequestId(std::move(future), req_id); +} + +int64_t +GenericClient::async_send_request_impl(const Request request, CallbackInfoVariant value) +{ + int64_t sequence_number; + std::lock_guard lock(pending_requests_mutex_); + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request, &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); + } + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); + return sequence_number; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp index 87ace1eb19..d526a6fac1 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -23,6 +23,10 @@ namespace rclcpp void GenericPublisher::publish(const rclcpp::SerializedMessage & message) { + TRACETOOLS_TRACEPOINT( + rclcpp_publish, + nullptr, + static_cast(&message.get_rcl_serialized_message())); auto return_code = rcl_publish_serialized_message( get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); @@ -68,6 +72,7 @@ void GenericPublisher::deserialize_message( void GenericPublisher::publish_loaned_message(void * loaned_message) { + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(loaned_message)); auto return_code = rcl_publish_loaned_message( get_publisher_handle().get(), loaned_message, NULL); diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index e6e61add24..ae28354b98 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -49,9 +49,9 @@ GenericSubscription::handle_message( void GenericSubscription::handle_serialized_message( const std::shared_ptr & message, - const rclcpp::MessageInfo &) + const rclcpp::MessageInfo & message_info) { - callback_(message); + any_callback_.dispatch(message, message_info); } void diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 627644e602..700985f620 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -23,16 +23,17 @@ namespace rclcpp { GuardCondition::GuardCondition( - rclcpp::Context::SharedPtr context, + const rclcpp::Context::SharedPtr & context, rcl_guard_condition_options_t guard_condition_options) -: context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} +: rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} { - if (!context_) { + if (!context) { throw std::invalid_argument("context argument unexpectedly nullptr"); } + rcl_ret_t ret = rcl_guard_condition_init( &this->rcl_guard_condition_, - context_->get_rcl_context().get(), + context->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); @@ -53,12 +54,6 @@ GuardCondition::~GuardCondition() } } -rclcpp::Context::SharedPtr -GuardCondition::get_context() const -{ - return context_; -} - rcl_guard_condition_t & GuardCondition::get_rcl_guard_condition() { @@ -97,19 +92,19 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) } void -GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) +GuardCondition::add_to_wait_set(rcl_wait_set_t & wait_set) { std::lock_guard lock(reentrant_mutex_); if (exchange_in_use_by_wait_set_state(true)) { - if (wait_set != wait_set_) { + if (&wait_set != wait_set_) { throw std::runtime_error("guard condition has already been added to a wait set."); } } else { - wait_set_ = wait_set; + wait_set_ = &wait_set; } - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL); + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, &this->rcl_guard_condition_, NULL); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( ret, "failed to add guard condition to wait set"); diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index efce4afeaf..831ffdf2ca 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -32,13 +32,24 @@ IntraProcessManager::~IntraProcessManager() {} uint64_t -IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) +IntraProcessManager::add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer) { std::unique_lock lock(mutex_); uint64_t pub_id = IntraProcessManager::get_next_unique_id(); publishers_[pub_id] = publisher; + if (publisher->is_durability_transient_local()) { + if (buffer) { + publisher_buffers_[pub_id] = buffer; + } else { + throw std::runtime_error( + "transient_local publisher needs to pass" + "a valid publisher buffer ptr when calling add_publisher()"); + } + } // Initialize the subscriptions storage for this publisher. pub_to_subs_[pub_id] = SplittedSubscriptions(); @@ -58,30 +69,6 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) return pub_id; } -uint64_t -IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription) -{ - std::unique_lock 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()); - } - } - - return sub_id; -} - void IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { @@ -112,6 +99,7 @@ IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) std::unique_lock lock(mutex_); publishers_.erase(intra_process_publisher_id); + publisher_buffers_.erase(intra_process_publisher_id); pub_to_subs_.erase(intra_process_publisher_id); } @@ -225,5 +213,52 @@ IntraProcessManager::can_communicate( return true; } +size_t +IntraProcessManager::lowest_available_capacity(const uint64_t intra_process_publisher_id) const +{ + size_t capacity = std::numeric_limits::max(); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling lowest_available_capacity for invalid or no longer existing publisher id"); + return 0u; + } + + if (publisher_it->second.take_shared_subscriptions.empty() && + publisher_it->second.take_ownership_subscriptions.empty()) + { + // no subscriptions available + return 0u; + } + + auto available_capacity = [this, &capacity](const uint64_t intra_process_subscription_id) + { + auto subscription_it = subscriptions_.find(intra_process_subscription_id); + if (subscription_it != subscriptions_.end()) { + auto subscription = subscription_it->second.lock(); + if (subscription) { + capacity = std::min(capacity, subscription->available_capacity()); + } + } else { + // Subscription is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling available_capacity for invalid or no longer existing subscription id"); + } + }; + + for (const auto sub_id : publisher_it->second.take_shared_subscriptions) { + available_capacity(sub_id); + } + + for (const auto sub_id : publisher_it->second.take_ownership_subscriptions) { + available_capacity(sub_id); + } + + return capacity; +} } // namespace experimental } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/logger.cpp b/rclcpp/src/rclcpp/logger.cpp index 874858b180..2e83c07013 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -17,6 +17,7 @@ #include #include "rcl_logging_interface/rcl_logging_interface.h" +#include "rcl/error_handling.h" #include "rcl/logging_rosout.h" #include "rclcpp/exceptions.hpp" @@ -80,10 +81,12 @@ Logger::get_child(const std::string & suffix) { std::lock_guard guard(*logging_mutex); rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str()); - if (RCL_RET_OK != rcl_ret) { + if (RCL_RET_NOT_FOUND == rcl_ret) { + rcl_reset_error(); + } else if (RCL_RET_OK != rcl_ret) { exceptions::throw_from_rcl_error( rcl_ret, "failed to call rcl_logging_rosout_add_sublogger", - rcutils_get_error_state(), rcutils_reset_error); + rcl_get_error_state(), rcl_reset_error); } } @@ -98,9 +101,7 @@ Logger::get_child(const std::string & suffix) logger_sublogger_pairname_ptr->second.c_str()); delete logger_sublogger_pairname_ptr; if (RCL_RET_OK != rcl_ret) { - exceptions::throw_from_rcl_error( - rcl_ret, "failed to call rcl_logging_rosout_remove_sublogger", - rcutils_get_error_state(), rcutils_reset_error); + rcl_reset_error(); } }); } diff --git a/rclcpp/src/rclcpp/logging_mutex.cpp b/rclcpp/src/rclcpp/logging_mutex.cpp index 308a21fe73..bbbe9bbeed 100644 --- a/rclcpp/src/rclcpp/logging_mutex.cpp +++ b/rclcpp/src/rclcpp/logging_mutex.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "rcutils/macros.h" diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 53e77dd796..1a68c7f108 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -23,6 +23,7 @@ #include "rcl/arguments.h" +#include "rclcpp/create_generic_client.hpp" #include "rclcpp/detail/qos_parameters.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" @@ -36,6 +37,7 @@ #include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/qos_overriding_options.hpp" @@ -109,6 +111,22 @@ create_effective_namespace(const std::string & node_namespace, const std::string } // namespace +/// Internal implementation to provide hidden and API/ABI stable changes to the Node. +/** + * This class is intended to be an "escape hatch" within a stable distribution, so that certain + * smaller features and bugfixes can be backported, having a place to put new members, while + * maintaining the ABI. + * + * This is not intended to be a parking place for new features, it should be used for backports + * only, left empty and unallocated in Rolling. + */ +class Node::NodeImpl +{ +public: + NodeImpl() = default; + ~NodeImpl() = default; +}; + Node::Node( const std::string & node_name, const NodeOptions & options) @@ -206,6 +224,12 @@ Node::Node( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, + node_services_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), sub_namespace_(""), @@ -246,7 +270,8 @@ Node::Node( node_waitables_(other.node_waitables_), node_options_(other.node_options_), sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)), - effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)) + effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)), + hidden_impl_(other.hidden_impl_) { // Validate new effective namespace. int validation_result; @@ -498,6 +523,18 @@ Node::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +size_t +Node::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +Node::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + std::vector Node::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const { @@ -591,6 +628,12 @@ Node::get_node_topics_interface() return node_topics_; } +rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr +Node::get_node_type_descriptions_interface() +{ + return node_type_descriptions_; +} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr Node::get_node_services_interface() { @@ -634,3 +677,20 @@ Node::get_node_options() const { return this->node_options_; } + +rclcpp::GenericClient::SharedPtr +Node::create_generic_client( + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_generic_client( + node_base_, + node_graph_, + node_services_, + service_name, + service_type, + qos, + group); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 6544d69214..5648290654 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -20,6 +20,9 @@ #include "rclcpp/node_interfaces/node_base.hpp" #include "rcl/arguments.h" +#include "rcl/node_type_cache.h" +#include "rcl/logging.h" +#include "rcl/logging_rosout.h" #include "rclcpp/exceptions.hpp" #include "rcutils/logging_macros.h" #include "rmw/validate_namespace.h" @@ -54,17 +57,12 @@ NodeBase::NodeBase( std::shared_ptr logging_mutex = get_global_logging_mutex(); rcl_ret_t ret; - { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): /rosout Qos should be reconfigurable. - // TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex, - // rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called - // here directly. - ret = rcl_node_init( - rcl_node.get(), - node_name.c_str(), namespace_.c_str(), - context_->get_rcl_context().get(), &rcl_node_options); - } + + // TODO(ivanpauno): /rosout Qos should be reconfigurable. + ret = rcl_node_init( + rcl_node.get(), + node_name.c_str(), namespace_.c_str(), + context_->get_rcl_context().get(), &rcl_node_options); if (ret != RCL_RET_OK) { if (ret == RCL_RET_NODE_INVALID_NAME) { rcl_reset_error(); // discard rcl_node_init error @@ -114,13 +112,29 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } + // The initialization for the rosout publisher + if (rcl_logging_rosout_enabled() && rcl_node_options.enable_rosout) { + std::lock_guard guard(*logging_mutex); + ret = rcl_logging_rosout_init_publisher_for_node(rcl_node.get()); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to initialize rosout publisher"); + } + } + node_handle_.reset( rcl_node.release(), - [logging_mutex](rcl_node_t * node) -> void { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, - // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called - // here directly. + [logging_mutex, rcl_node_options](rcl_node_t * node) -> void { + { + std::lock_guard guard(*logging_mutex); + if (rcl_logging_rosout_enabled() && rcl_node_options.enable_rosout) { + rcl_ret_t ret = rcl_logging_rosout_fini_publisher_for_node(node); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rosout publisher: %s", rcl_get_error_string().str); + } + } + } if (rcl_node_fini(node) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -204,14 +218,9 @@ NodeBase::create_callback_group( rclcpp::CallbackGroupType group_type, bool automatically_add_to_executor_with_node) { - auto weak_context = this->get_context()->weak_from_this(); - auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr { - return weak_context.lock(); - }; - auto group = std::make_shared( group_type, - get_node_context, + context_->weak_from_this(), automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 1228703cb6..f6e9e1fda0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -498,6 +498,50 @@ NodeGraph::count_subscribers(const std::string & topic_name) const return count; } +size_t +NodeGraph::count_clients(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_clients(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count clients: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + +size_t +NodeGraph::count_services(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_services(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count services: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + const rcl_guard_condition_t * NodeGraph::get_graph_guard_condition() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c8bb62e238..922ce9e4d1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -1038,37 +1038,50 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity // using "." for now const char * separator = "."; - for (auto & kv : parameters_) { - bool get_all = (prefixes.size() == 0) && - ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth)); - bool prefix_matches = std::any_of( - prefixes.cbegin(), prefixes.cend(), - [&kv, &depth, &separator](const std::string & prefix) { - if (kv.first == prefix) { - return true; - } else if (kv.first.find(prefix + separator) == 0) { - size_t length = prefix.length(); - std::string substr = kv.first.substr(length); - // Cast as unsigned integer to avoid warning - return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(substr.begin(), substr.end(), *separator)) < depth); - } - return false; - }); - if (get_all || prefix_matches) { - result.names.push_back(kv.first); - size_t last_separator = kv.first.find_last_of(separator); - if (std::string::npos != last_separator) { - std::string prefix = kv.first.substr(0, last_separator); - if ( - std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == - result.prefixes.cend()) - { - result.prefixes.push_back(prefix); + + auto separators_less_than_depth = [&depth, &separator](const std::string & str) -> bool { + return static_cast(std::count(str.begin(), str.end(), *separator)) < depth; + }; + + bool recursive = (prefixes.size() == 0) && + (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); + + for (const std::pair & kv : parameters_) { + if (!recursive) { + bool get_all = (prefixes.size() == 0) && separators_less_than_depth(kv.first); + if (!get_all) { + bool prefix_matches = std::any_of( + prefixes.cbegin(), prefixes.cend(), + [&kv, &depth, &separator, &separators_less_than_depth](const std::string & prefix) { + if (kv.first == prefix) { + return true; + } else if (kv.first.find(prefix + separator) == 0) { + if (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) { + return true; + } + std::string substr = kv.first.substr(prefix.length() + 1); + return separators_less_than_depth(substr); + } + return false; + }); + + if (!prefix_matches) { + continue; } } } + + result.names.push_back(kv.first); + size_t last_separator = kv.first.find_last_of(separator); + if (std::string::npos != last_separator) { + std::string prefix = kv.first.substr(0, last_separator); + if ( + std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == + result.prefixes.cend()) + { + result.prefixes.push_back(prefix); + } + } } return result; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index e9c4a5266e..fdd4e83780 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -32,8 +32,7 @@ NodeServices::add_service( { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create service, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("service"); } } else { group = node_base_->get_default_callback_group(); @@ -58,8 +57,7 @@ NodeServices::add_client( { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create client, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("client"); } } else { group = node_base_->get_default_callback_group(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 96097def22..29d3125e1f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -34,8 +34,7 @@ NodeTimers::add_timer( { if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create timer, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("timer"); } } else { callback_group = node_base_->get_default_callback_group(); @@ -50,7 +49,7 @@ NodeTimers::add_timer( std::string("failed to notify wait set on timer creation: ") + ex.what()); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_timer_link_node, static_cast(timer->get_timer_handle().get()), static_cast(node_base_->get_rcl_node_handle())); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 659129d62c..ce71036b93 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -58,7 +58,7 @@ NodeTopics::add_publisher( // Assign to a group. if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - throw std::runtime_error("Cannot create publisher, callback group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("publisher"); } } else { callback_group = node_base_->get_default_callback_group(); @@ -97,8 +97,7 @@ NodeTopics::add_subscription( // Assign to a group. if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create subscription, callback group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("subscription"); } } else { callback_group = node_base_->get_default_callback_group(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp new file mode 100644 index 0000000000..fdac4652e0 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -0,0 +1,153 @@ +// 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 +#include +#include + +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" +#include "rclcpp/parameter_client.hpp" + +#include "type_description_interfaces/srv/get_type_description.h" + +namespace +{ +// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. +struct GetTypeDescription__C +{ + using Request = type_description_interfaces__srv__GetTypeDescription_Request; + using Response = type_description_interfaces__srv__GetTypeDescription_Response; + using Event = type_description_interfaces__srv__GetTypeDescription_Event; +}; +} // namespace + +// Helper function for C typesupport. +namespace rosidl_typesupport_cpp +{ +template<> +rosidl_service_type_support_t const * +get_service_type_support_handle() +{ + return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); +} +} // namespace rosidl_typesupport_cpp + +namespace rclcpp +{ +namespace node_interfaces +{ + +class NodeTypeDescriptions::NodeTypeDescriptionsImpl +{ +public: + using ServiceT = GetTypeDescription__C; + + rclcpp::Logger logger_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::Service::SharedPtr type_description_srv_; + + NodeTypeDescriptionsImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) + : logger_(node_logging->get_logger()), + node_base_(node_base) + { + const std::string enable_param_name = "start_type_description_service"; + + bool enabled = false; + try { + auto enable_param = node_parameters->declare_parameter( + enable_param_name, + rclcpp::ParameterValue(true), + rcl_interfaces::msg::ParameterDescriptor() + .set__name(enable_param_name) + .set__type(rclcpp::PARAMETER_BOOL) + .set__description("Start the ~/get_type_description service for this node.") + .set__read_only(true)); + enabled = enable_param.get(); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { + RCLCPP_ERROR(logger_, "%s", exc.what()); + throw; + } + + if (enabled) { + auto * rcl_node = node_base->get_rcl_node_handle(); + std::shared_ptr rcl_srv( + new rcl_service_t, + [rcl_node, logger = this->logger_](rcl_service_t * service) + { + if (rcl_service_fini(service, rcl_node) != RCL_RET_OK) { + RCLCPP_ERROR( + logger, + "Error in destruction of rcl service handle [~/get_type_description]: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *rcl_srv = rcl_get_zero_initialized_service(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_srv.get(), rcl_node); + + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR( + logger_, "Failed to initialize ~/get_type_description service: %s", + rcl_get_error_string().str); + throw std::runtime_error( + "Failed to initialize ~/get_type_description service."); + } + + rclcpp::AnyServiceCallback cb; + cb.set( + [this]( + std::shared_ptr header, + std::shared_ptr request, + std::shared_ptr response + ) { + rcl_node_type_description_service_handle_request( + node_base_->get_rcl_node_handle(), + header.get(), + request.get(), + response.get()); + }); + + type_description_srv_ = std::make_shared>( + node_base_->get_shared_rcl_node_handle(), + rcl_srv, + cb); + node_services->add_service( + std::dynamic_pointer_cast(type_description_srv_), + nullptr); + } + } +}; + +NodeTypeDescriptions::NodeTypeDescriptions( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) +: impl_(new NodeTypeDescriptionsImpl( + node_base, + node_logging, + node_parameters, + node_services)) +{} + +NodeTypeDescriptions::~NodeTypeDescriptions() +{} + +} // namespace node_interfaces +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 02a9de82b0..96eb8df9cf 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -32,8 +32,7 @@ NodeWaitables::add_waitable( { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacobperron): use custom exception - throw std::runtime_error("Cannot create waitable, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("waitable"); } } else { group = node_base_->get_default_callback_group(); diff --git a/rclcpp/src/rclcpp/parameter_value.cpp b/rclcpp/src/rclcpp/parameter_value.cpp index e88b225479..ee46e77673 100644 --- a/rclcpp/src/rclcpp/parameter_value.cpp +++ b/rclcpp/src/rclcpp/parameter_value.cpp @@ -129,8 +129,7 @@ ParameterValue::ParameterValue(const rcl_interfaces::msg::ParameterValue & value case PARAMETER_NOT_SET: break; default: - // TODO(wjwwood): use custom exception - throw std::runtime_error("Unknown type: " + std::to_string(value.type)); + throw rclcpp::exceptions::UnknownTypeError(std::to_string(value.type)); } } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 698db2d559..111246ed96 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -270,6 +270,13 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } +bool +PublisherBase::is_durability_transient_local() const +{ + return rcl_publisher_get_actual_qos(publisher_handle_.get())->durability == + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +} + rclcpp::QoS PublisherBase::get_actual_qos() const { @@ -384,3 +391,22 @@ std::vector PublisherBase::get_network_flow_endpoin return network_flow_endpoint_vector; } + +size_t PublisherBase::lowest_available_ipm_capacity() const +{ + if (!intra_process_is_enabled_) { + return 0u; + } + + auto ipm = weak_ipm_.lock(); + + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died for a publisher."); + return 0u; + } + + return ipm->lowest_available_capacity(intra_process_publisher_id_); +} diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp new file mode 100644 index 0000000000..7c98d6d4fe --- /dev/null +++ b/rclcpp/src/rclcpp/rate.cpp @@ -0,0 +1,106 @@ +// 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/rate.hpp" + +#include +#include + +namespace rclcpp +{ + +Rate::Rate( + const double rate, Clock::SharedPtr clock) +: clock_(clock), period_(0, 0), last_interval_(clock_->now()) +{ + if (rate <= 0.0) { + throw std::invalid_argument{"rate must be greater than 0"}; + } + period_ = Duration::from_seconds(1.0 / rate); +} + +Rate::Rate( + const Duration & period, Clock::SharedPtr clock) +: clock_(clock), period_(period), last_interval_(clock_->now()) +{ + if (period <= Duration(0, 0)) { + throw std::invalid_argument{"period must be greater than 0"}; + } +} + +bool +Rate::sleep() +{ + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (next_interval <= now) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + clock_->sleep_for(time_to_sleep); + return true; +} + +bool +Rate::is_steady() const +{ + return clock_->get_clock_type() == RCL_STEADY_TIME; +} + +rcl_clock_type_t +Rate::get_type() const +{ + return clock_->get_clock_type(); +} + +void +Rate::reset() +{ + last_interval_ = clock_->now(); +} + +std::chrono::nanoseconds +Rate::period() const +{ + return std::chrono::nanoseconds(period_.nanoseconds()); +} + +WallRate::WallRate(const double rate) +: Rate(rate, std::make_shared(RCL_STEADY_TIME)) +{} + +WallRate::WallRate(const Duration & period) +: Rate(period, std::make_shared(RCL_STEADY_TIME)) +{} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index 7085c63bdf..cf26d06df4 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -113,7 +113,7 @@ SignalHandler::get_logger() SignalHandler & SignalHandler::get_global_signal_handler() { - static SignalHandler signal_handler; + static SignalHandler & signal_handler = *new SignalHandler(); return signal_handler; } diff --git a/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp/src/rclcpp/signal_handler.hpp index 0d3c399ccb..db608b0d10 100644 --- a/rclcpp/src/rclcpp/signal_handler.hpp +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -75,7 +75,7 @@ class SignalHandler final bool install(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); - /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated singal handling + /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated signal handling /// thread. /** * Also restores the previous signal handler. @@ -189,7 +189,7 @@ class SignalHandler final // Whether or not a signal has been received. std::atomic_bool signal_received_ = false; - // A thread to which singal handling tasks are deferred. + // A thread to which signal handling tasks are deferred. std::thread signal_handler_thread_; // A mutex used to synchronize the install() and uninstall() methods. diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 18ccab0eb0..f7c238a910 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -52,7 +52,7 @@ SubscriptionBase::SubscriptionBase( intra_process_subscription_id_(0), event_callbacks_(event_callbacks), type_support_(type_support_handle), - delivered_message_type_(delivered_message_kind) + delivered_message_kind_(delivered_message_kind) { auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { @@ -218,7 +218,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes &message_info_out.get_rmw_message_info(), nullptr // rmw_subscription_allocation_t is unused here ); - TRACEPOINT(rclcpp_take, static_cast(message_out)); + TRACETOOLS_TRACEPOINT(rclcpp_take, static_cast(message_out)); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { @@ -244,6 +244,9 @@ SubscriptionBase::take_serialized( &message_out.get_rcl_serialized_message(), &message_info_out.get_rmw_message_info(), nullptr); + TRACETOOLS_TRACEPOINT( + rclcpp_take, + static_cast(&message_out.get_rcl_serialized_message())); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { @@ -261,13 +264,13 @@ SubscriptionBase::get_message_type_support_handle() const bool SubscriptionBase::is_serialized() const { - return delivered_message_type_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; + return delivered_message_kind_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; } rclcpp::DeliveredMessageKind -SubscriptionBase::get_subscription_type() const +SubscriptionBase::get_delivered_message_kind() const { - return delivered_message_type_; + return delivered_message_kind_; } size_t @@ -298,7 +301,20 @@ SubscriptionBase::setup_intra_process( bool SubscriptionBase::can_loan_messages() const { - return rcl_subscription_can_loan_messages(subscription_handle_.get()); + bool retval = rcl_subscription_can_loan_messages(subscription_handle_.get()); + if (retval) { + // TODO(clalancette): The loaned message interface is currently not safe to use with + // shared_ptr callbacks. If a user takes a copy of the shared_ptr, it can get freed from + // underneath them via rcl_return_loaned_message_from_subscription(). The correct solution is + // to return the loaned message in a custom deleter, but that needs to be carefully handled + // with locking. Warn the user about this until we fix it. + RCLCPP_WARN_ONCE( + this->node_logger_, + "Loaned messages are only safe with const ref subscription callbacks. " + "If you are using any other kind of subscriptions, " + "set the ROS_DISABLE_LOANED_MESSAGES environment variable to 1 (the default)."); + } + return retval; } rclcpp::Waitable::SharedPtr diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index b13123b65b..f9d19da8c5 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -18,9 +18,9 @@ using rclcpp::experimental::SubscriptionIntraProcessBase; void -SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t & wait_set) { - detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); } const char * @@ -34,3 +34,9 @@ SubscriptionIntraProcessBase::get_actual_qos() const { return qos_profile_; } + +bool +SubscriptionIntraProcessBase::is_durability_transient_local() const +{ + return qos_profile_.durability() == rclcpp::DurabilityPolicy::TransientLocal; +} diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index 556a5e69ad..978651516c 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -65,6 +65,8 @@ Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) Time::Time(const Time & rhs) = default; +Time::Time(Time && rhs) noexcept = default; + Time::Time( const builtin_interfaces::msg::Time & time_msg, rcl_clock_type_t clock_type) @@ -84,23 +86,11 @@ Time::Time(const rcl_time_point_t & time_point) // noop } -Time::~Time() -{ -} +Time::~Time() = default; Time::operator builtin_interfaces::msg::Time() const { - builtin_interfaces::msg::Time msg_time; - constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); - const auto result = std::div(rcl_time_.nanoseconds, kRemainder); - if (result.rem >= 0) { - msg_time.sec = static_cast(result.quot); - msg_time.nanosec = static_cast(result.rem); - } else { - msg_time.sec = static_cast(result.quot - 1); - msg_time.nanosec = static_cast(kRemainder + result.rem); - } - return msg_time; + return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds); } Time & @@ -113,6 +103,9 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg) return *this; } +Time & +Time::operator=(Time && rhs) noexcept = default; + bool Time::operator==(const rclcpp::Time & rhs) const { @@ -276,10 +269,25 @@ Time::operator-=(const rclcpp::Duration & rhs) } Time -Time::max() +Time::max(rcl_clock_type_t clock_type) { - return Time(std::numeric_limits::max(), 999999999); + return Time(std::numeric_limits::max(), 999999999, clock_type); } +builtin_interfaces::msg::Time +convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point) +{ + builtin_interfaces::msg::Time ret; + constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); + const auto result = std::div(time_point, kRemainder); + if (result.rem >= 0) { + ret.sec = static_cast(result.quot); + ret.nanosec = static_cast(result.rem); + } else { + ret.sec = static_cast(result.quot - 1); + ret.nanosec = static_cast(kRemainder + result.rem); + } + return ret; +} } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 7f3b3b9536..465ceaf5a7 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -54,9 +55,7 @@ class ClocksState final ros_time_active_ = true; // Update all attached clocks to zero or last recorded time - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(last_time_msg_, true, *it); - } + set_all_clocks(last_time_msg_, true); } // An internal method to use in the clock callback that iterates and disables all clocks @@ -71,11 +70,8 @@ class ClocksState final ros_time_active_ = false; // Update all attached clocks - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - auto msg = std::make_shared(); - set_clock(msg, false, *it); - } + auto msg = std::make_shared(); + set_all_clocks(msg, false); } // Check if ROS time is active @@ -95,7 +91,7 @@ class ClocksState final } } std::lock_guard guard(clock_list_lock_); - associated_clocks_.push_back(clock); + associated_clocks_.insert(clock); // Set the clock to zero unless there's a recently received message set_clock(last_time_msg_, ros_time_active_, clock); } @@ -104,10 +100,8 @@ class ClocksState final void detachClock(rclcpp::Clock::SharedPtr clock) { std::lock_guard guard(clock_list_lock_); - auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); - if (result != associated_clocks_.end()) { - associated_clocks_.erase(result); - } else { + auto removed = associated_clocks_.erase(clock); + if (removed == 0) { RCLCPP_ERROR(logger_, "failed to remove clock"); } } @@ -184,8 +178,8 @@ class ClocksState final // A lock to protect iterating the associated_clocks_ field. std::mutex clock_list_lock_; - // A vector to store references to associated clocks. - std::vector associated_clocks_; + // An unordered_set to store references to associated clocks. + std::unordered_set associated_clocks_; // Local storage of validity of ROS time // This is needed when new clocks are added. @@ -242,6 +236,7 @@ class TimeSource::NodeState final rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) { + std::lock_guard guard(node_base_lock_); node_base_ = node_base_interface; node_topics_ = node_topics_interface; node_graph_ = node_graph_interface; @@ -286,17 +281,14 @@ class TimeSource::NodeState final parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, [this](std::shared_ptr event) { - if (node_base_ != nullptr) { - this->on_parameter_event(event); - } - // Do nothing if node_base_ is nullptr because it means the TimeSource is now - // without an attached node + this->on_parameter_event(event); }); } // Detach the attached node void detachNode() { + std::lock_guard guard(node_base_lock_); // destroy_clock_sub() *must* be first here, to ensure that the executor // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); @@ -333,6 +325,7 @@ class TimeSource::NodeState final std::thread clock_executor_thread_; // Preserve the node reference + std::mutex node_base_lock_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr}; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr}; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr}; @@ -470,6 +463,14 @@ class TimeSource::NodeState final // Callback for parameter updates void on_parameter_event(std::shared_ptr event) { + std::lock_guard guard(node_base_lock_); + + if (node_base_ == nullptr) { + // Do nothing if node_base_ is nullptr because it means the TimeSource is now + // without an attached node + return; + } + // Filter out events on 'use_sim_time' parameter instances in other nodes. if (event->node != node_base_->get_fully_qualified_name()) { return; diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 59ddd5e8dd..0dceb6b8d7 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -32,7 +32,8 @@ using rclcpp::TimerBase; TimerBase::TimerBase( rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + bool autostart) : clock_(clock), timer_handle_(nullptr) { if (nullptr == context) { @@ -64,9 +65,9 @@ TimerBase::TimerBase( rcl_clock_t * clock_handle = clock_->get_clock_handle(); { std::lock_guard clock_guard(clock_->get_clock_mutex()); - rcl_ret_t ret = rcl_timer_init( - timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, - rcl_get_default_allocator()); + rcl_ret_t ret = rcl_timer_init2( + timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), + nullptr, rcl_get_default_allocator(), autostart); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); } diff --git a/rclcpp/src/rclcpp/typesupport_helpers.cpp b/rclcpp/src/rclcpp/typesupport_helpers.cpp index 7286c35baa..04dd8c4c9f 100644 --- a/rclcpp/src/rclcpp/typesupport_helpers.cpp +++ b/rclcpp/src/rclcpp/typesupport_helpers.cpp @@ -91,20 +91,12 @@ extract_type_identifier(const std::string & full_type) return std::make_tuple(package_name, middle_module, type_name); } -} // anonymous namespace - -std::shared_ptr -get_typesupport_library(const std::string & type, const std::string & typesupport_identifier) -{ - auto package_name = std::get<0>(extract_type_identifier(type)); - auto library_path = get_typesupport_library_path(package_name, typesupport_identifier); - return std::make_shared(library_path); -} - -const rosidl_message_type_support_t * -get_typesupport_handle( +const void * get_typesupport_handle_impl( const std::string & type, const std::string & typesupport_identifier, + const std::string & typesupport_name, + const std::string & symbol_part_name, + const std::string & middle_module_additional, rcpputils::SharedLibrary & library) { std::string package_name; @@ -112,19 +104,23 @@ get_typesupport_handle( std::string type_name; std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); - auto mk_error = [&package_name, &type_name](auto reason) { + if (middle_module.empty()) { + middle_module = middle_module_additional; + } + + auto mk_error = [&package_name, &type_name, &typesupport_name](auto reason) { std::stringstream rcutils_dynamic_loading_error; rcutils_dynamic_loading_error << - "Something went wrong loading the typesupport library for message type " << package_name << + "Something went wrong loading the typesupport library for " << + typesupport_name << " type " << package_name << "/" << type_name << ". " << reason; return rcutils_dynamic_loading_error.str(); }; try { - std::string symbol_name = typesupport_identifier + "__get_message_type_support_handle__" + - package_name + "__" + (middle_module.empty() ? "msg" : middle_module) + "__" + type_name; - - const rosidl_message_type_support_t * (* get_ts)() = nullptr; + std::string symbol_name = typesupport_identifier + symbol_part_name + + package_name + "__" + middle_module + "__" + type_name; + const void * (* get_ts)() = nullptr; // This will throw runtime_error if the symbol was not found. get_ts = reinterpret_cast(library.get_symbol(symbol_name)); return get_ts(); @@ -133,4 +129,52 @@ get_typesupport_handle( } } +} // anonymous namespace + +std::shared_ptr +get_typesupport_library(const std::string & type, const std::string & typesupport_identifier) +{ + auto package_name = std::get<0>(extract_type_identifier(type)); + auto library_path = get_typesupport_library_path(package_name, typesupport_identifier); + return std::make_shared(library_path); +} + +const rosidl_message_type_support_t * get_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + return get_message_typesupport_handle(type, typesupport_identifier, library); +} + +const rosidl_message_type_support_t * get_message_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + static const std::string typesupport_name = "message"; + static const std::string symbol_part_name = "__get_message_type_support_handle__"; + static const std::string middle_module_additional = "msg"; + + return static_cast(get_typesupport_handle_impl( + type, typesupport_identifier, typesupport_name, symbol_part_name, + middle_module_additional, library + )); +} + +const rosidl_service_type_support_t * get_service_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + static const std::string typesupport_name = "service"; + static const std::string symbol_part_name = "__get_service_type_support_handle__"; + static const std::string middle_module_additional = "srv"; + + return static_cast(get_typesupport_handle_impl( + type, typesupport_identifier, typesupport_name, symbol_part_name, + middle_module_additional, library + )); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 6c1284cb22..43b562481c 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -86,3 +86,82 @@ Waitable::clear_on_ready_callback() "Custom waitables should override clear_on_ready_callback if they " "want to use it and make sure to call it on the waitable destructor."); } + +void +Waitable::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + this->add_to_wait_set(*wait_set); +} + +void +Waitable::add_to_wait_set(rcl_wait_set_t & wait_set) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + this->add_to_wait_set(&wait_set); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + +bool +Waitable::is_ready(rcl_wait_set_t * wait_set) +{ + const rcl_wait_set_t & const_wait_set_ref = *wait_set; + return this->is_ready(const_wait_set_ref); +} + +bool +Waitable::is_ready(const rcl_wait_set_t & wait_set) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // note this const cast is only required to support a deprecated function + return this->is_ready(&const_cast(wait_set)); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + +void +Waitable::execute(std::shared_ptr & data) +{ + const std::shared_ptr & const_data = data; + this->execute(const_data); +} + +void +Waitable::execute(const std::shared_ptr & data) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // note this const cast is only required to support a deprecated function + this->execute(const_cast &>(data)); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} diff --git a/rclcpp/test/benchmark/CMakeLists.txt b/rclcpp/test/benchmark/CMakeLists.txt index a88c43e3b2..6b93711df2 100644 --- a/rclcpp/test/benchmark/CMakeLists.txt +++ b/rclcpp/test/benchmark/CMakeLists.txt @@ -7,14 +7,12 @@ find_package(performance_test_fixture REQUIRED) add_performance_test(benchmark_client benchmark_client.cpp) if(TARGET benchmark_client) - target_link_libraries(benchmark_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_client test_msgs rcl_interfaces) + target_link_libraries(benchmark_client ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() add_performance_test(benchmark_executor benchmark_executor.cpp) if(TARGET benchmark_executor) - target_link_libraries(benchmark_executor ${PROJECT_NAME}) - ament_target_dependencies(benchmark_executor test_msgs) + target_link_libraries(benchmark_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() add_performance_test(benchmark_init_shutdown benchmark_init_shutdown.cpp) @@ -39,6 +37,5 @@ endif() add_performance_test(benchmark_service benchmark_service.cpp) if(TARGET benchmark_service) - target_link_libraries(benchmark_service ${PROJECT_NAME}) - ament_target_dependencies(benchmark_service test_msgs rcl_interfaces) + target_link_libraries(benchmark_service ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 652007b589..65bb3a1007 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -362,42 +362,3 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark } } } - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_executor_entities_collector_execute)(benchmark::State & st) -{ - rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ = - std::make_shared(); - entities_collector_->add_node(node->get_node_base_interface()); - - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - auto shared_context = node->get_node_base_interface()->get_context(); - rcl_context_t * context = shared_context->get_rcl_context().get(); - rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - RCPPUTILS_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_wait_set_fini(&wait_set); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - }); - - auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); - rclcpp::GuardCondition guard_condition(shared_context); - - entities_collector_->init(&wait_set, memory_strategy); - RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - std::shared_ptr data = entities_collector_->take_data(); - entities_collector_->execute(data); - } -} diff --git a/rclcpp/test/msg/LargeMessage.msg b/rclcpp/test/msg/LargeMessage.msg new file mode 100644 index 0000000000..1e383c0bae --- /dev/null +++ b/rclcpp/test/msg/LargeMessage.msg @@ -0,0 +1,3 @@ +# A message with a size larger than the default Linux stack size +uint8[10485760] data +uint64 size diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index dcb1d36d8e..c2e6b2bfe4 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -6,6 +6,7 @@ add_definitions(-DTEST_RESOURCES_DIRECTORY="${TEST_RESOURCES_DIRECTORY}") rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/Header.msg + ../msg/LargeMessage.msg ../msg/MessageWithHeader.msg ../msg/String.msg DEPENDENCIES builtin_interfaces @@ -34,71 +35,60 @@ if(TARGET test_exceptions) target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) endif() -# Increasing timeout because connext can take a long time to destroy nodes -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 -ament_add_gtest( - test_allocator_memory_strategy - strategies/test_allocator_memory_strategy.cpp - TIMEOUT 360) +ament_add_gtest(test_allocator_memory_strategy strategies/test_allocator_memory_strategy.cpp) if(TARGET test_allocator_memory_strategy) - ament_target_dependencies(test_allocator_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME}) + target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) endif() ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp) if(TARGET test_message_pool_memory_strategy) - ament_target_dependencies(test_message_pool_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME}) + target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_any_service_callback test_any_service_callback.cpp) if(TARGET test_any_service_callback) - ament_target_dependencies(test_any_service_callback - "test_msgs" - ) - target_link_libraries(test_any_service_callback ${PROJECT_NAME}) + target_link_libraries(test_any_service_callback ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp) if(TARGET test_any_subscription_callback) - ament_target_dependencies(test_any_subscription_callback - "test_msgs" - ) - target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) + target_link_libraries(test_any_subscription_callback ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_client test_client.cpp) if(TARGET test_client) - ament_target_dependencies(test_client - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_client ${PROJECT_NAME} mimick) + target_link_libraries(test_client ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp) +if(TARGET test_copy_all_parameter_values) + target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME}) endif() ament_add_gtest(test_create_timer test_create_timer.cpp) if(TARGET test_create_timer) - ament_target_dependencies(test_create_timer - "rcl_interfaces" - "rmw" - "rcl" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_create_timer ${PROJECT_NAME}) target_include_directories(test_create_timer PRIVATE ./) endif() +ament_add_gtest(test_generic_client test_generic_client.cpp) +if(TARGET test_generic_client) + target_link_libraries(test_generic_client ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() +ament_add_gtest(test_client_common test_client_common.cpp) +if(TARGET test_client_common) + target_link_libraries(test_client_common ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() ament_add_gtest(test_create_subscription test_create_subscription.cpp) if(TARGET test_create_subscription) - target_link_libraries(test_create_subscription ${PROJECT_NAME}) - ament_target_dependencies(test_create_subscription - "test_msgs" - ) + target_link_libraries(test_create_subscription ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() function(test_add_callback_groups_to_executor_for_rmw_implementation) set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) @@ -107,32 +97,17 @@ function(test_add_callback_groups_to_executor_for_rmw_implementation) TIMEOUT 120 ) if(TARGET test_add_callback_groups_to_executor${target_suffix}) - target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME}) - ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix} - "test_msgs" - ) + target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation) ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) if(TARGET test_expand_topic_or_service_name) - ament_target_dependencies(test_expand_topic_or_service_name - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick) + target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) endif() ament_add_gtest(test_function_traits test_function_traits.cpp) if(TARGET test_function_traits) - target_include_directories(test_function_traits PUBLIC ../../include) - ament_target_dependencies(test_function_traits - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) + target_link_libraries(test_function_traits ${PROJECT_NAME}) endif() ament_add_gtest( test_future_return_code @@ -142,72 +117,33 @@ if(TARGET test_future_return_code) endif() ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) if(TARGET test_intra_process_manager) - ament_target_dependencies(test_intra_process_manager - "rcl" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME} ${rcl_interfaces_TARGETS} rmw::rmw) endif() ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp) if(TARGET test_intra_process_manager_with_allocators) - ament_target_dependencies(test_intra_process_manager_with_allocators - "test_msgs" - ) - target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME}) + target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp) if(TARGET test_ring_buffer_implementation) - ament_target_dependencies(test_ring_buffer_implementation - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) endif() ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp) if(TARGET test_intra_process_buffer) - ament_target_dependencies(test_intra_process_buffer - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) endif() ament_add_gtest(test_loaned_message test_loaned_message.cpp) -ament_target_dependencies(test_loaned_message - "test_msgs" -) -target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick) +target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) ament_add_gtest(test_memory_strategy test_memory_strategy.cpp) -ament_target_dependencies(test_memory_strategy - "test_msgs" -) -target_link_libraries(test_memory_strategy ${PROJECT_NAME}) +target_link_libraries(test_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) -ament_target_dependencies(test_message_memory_strategy - "test_msgs" -) -target_link_libraries(test_message_memory_strategy ${PROJECT_NAME}) +target_link_libraries(test_message_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) ament_add_gtest(test_node test_node.cpp TIMEOUT 240) if(TARGET test_node) - ament_target_dependencies(test_node - "rcl_interfaces" - "rcpputils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_node ${PROJECT_NAME} mimick) + target_link_libraries(test_node ${PROJECT_NAME} mimick rcpputils::rcpputils rmw::rmw ${test_msgs_TARGETS}) endif() ament_add_gtest(test_node_interfaces__get_node_interfaces @@ -218,7 +154,7 @@ endif() ament_add_gtest(test_node_interfaces__node_base node_interfaces/test_node_base.cpp) if(TARGET test_node_interfaces__node_base) - target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) endif() ament_add_gtest(test_node_interfaces__node_clock node_interfaces/test_node_clock.cpp) @@ -229,43 +165,42 @@ ament_add_gtest(test_node_interfaces__node_graph node_interfaces/test_node_graph.cpp TIMEOUT 120) if(TARGET test_node_interfaces__node_graph) - ament_target_dependencies( - test_node_interfaces__node_graph - "test_msgs") - target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_node_interfaces__node_interfaces node_interfaces/test_node_interfaces.cpp) if(TARGET test_node_interfaces__node_interfaces) - target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME}) endif() ament_add_gtest(test_node_interfaces__node_parameters node_interfaces/test_node_parameters.cpp) if(TARGET test_node_interfaces__node_parameters) - target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils) endif() ament_add_gtest(test_node_interfaces__node_services node_interfaces/test_node_services.cpp) if(TARGET test_node_interfaces__node_services) - target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__node_timers node_interfaces/test_node_timers.cpp) if(TARGET test_node_interfaces__node_timers) - target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__node_topics node_interfaces/test_node_topics.cpp) if(TARGET test_node_interfaces__node_topics) - ament_target_dependencies( - test_node_interfaces__node_topics - "test_msgs") - target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_node_interfaces__node_type_descriptions + node_interfaces/test_node_type_descriptions.cpp) +if(TARGET test_node_interfaces__node_type_descriptions) + target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME}) endif() ament_add_gtest(test_node_interfaces__node_waitables node_interfaces/test_node_waitables.cpp) if(TARGET test_node_interfaces__node_waitables) - target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__test_template_utils # Compile time test node_interfaces/detail/test_template_utils.cpp) @@ -296,82 +231,43 @@ endif() ament_add_gtest(test_node_global_args test_node_global_args.cpp) if(TARGET test_node_global_args) - ament_target_dependencies(test_node_global_args - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_node_global_args ${PROJECT_NAME}) endif() ament_add_gtest(test_node_options test_node_options.cpp) if(TARGET test_node_options) - ament_target_dependencies(test_node_options "rcl") - target_link_libraries(test_node_options ${PROJECT_NAME} mimick) + target_link_libraries(test_node_options ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_init_options test_init_options.cpp) if(TARGET test_init_options) - ament_target_dependencies(test_init_options "rcl") - target_link_libraries(test_init_options ${PROJECT_NAME} mimick) + target_link_libraries(test_init_options ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gmock(test_parameter_client test_parameter_client.cpp) if(TARGET test_parameter_client) - ament_target_dependencies(test_parameter_client - "rcl_interfaces" - ) - target_link_libraries(test_parameter_client ${PROJECT_NAME}) + target_link_libraries(test_parameter_client ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_parameter_service test_parameter_service.cpp) if(TARGET test_parameter_service) - ament_target_dependencies(test_parameter_service - "rcl_interfaces" - ) target_link_libraries(test_parameter_service ${PROJECT_NAME}) endif() ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp) if(TARGET test_parameter_events_filter) - ament_target_dependencies(test_parameter_events_filter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) + target_link_libraries(test_parameter_events_filter ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_parameter test_parameter.cpp) if(TARGET test_parameter) - ament_target_dependencies(test_parameter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_parameter ${PROJECT_NAME}) endif() ament_add_gtest(test_parameter_event_handler test_parameter_event_handler.cpp) if(TARGET test_parameter_event_handler) - ament_target_dependencies(test_parameter_event_handler - "rcl_interfaces" - "rmw" - "rosidl_generator_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_parameter_event_handler ${PROJECT_NAME}) endif() ament_add_gtest(test_parameter_map test_parameter_map.cpp) if(TARGET test_parameter_map) - target_link_libraries(test_parameter_map ${PROJECT_NAME}) + target_link_libraries(test_parameter_map ${PROJECT_NAME} rcl::rcl rcl_yaml_param_parser::rcl_yaml_param_parser rcutils::rcutils) endif() ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120) if(TARGET test_publisher) - ament_target_dependencies(test_publisher - "rcl" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher ${PROJECT_NAME} mimick) + target_link_libraries(test_publisher ${PROJECT_NAME} mimick rcl::rcl rcutils::rcutils ${test_msgs_TARGETS}) endif() set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") @@ -401,32 +297,22 @@ ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscrip APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_subscription_publisher_with_same_type_adapter) - ament_target_dependencies(test_subscription_publisher_with_same_type_adapter - "statistics_msgs" - ) target_link_libraries(test_subscription_publisher_with_same_type_adapter ${PROJECT_NAME} - ${cpp_typesupport_target}) + ${cpp_typesupport_target} + ${statistics_msgs_TARGETS} + ) endif() ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) - ament_target_dependencies(test_publisher_subscription_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) + target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_qos test_qos.cpp) if(TARGET test_qos) - ament_target_dependencies(test_qos - "rmw" - ) target_link_libraries(test_qos ${PROJECT_NAME} + rmw::rmw ) endif() function(test_generic_pubsub_for_rmw_implementation) @@ -435,12 +321,7 @@ function(test_generic_pubsub_for_rmw_implementation) ENV ${rmw_implementation_env_var} ) if(TARGET test_generic_pubsub${target_suffix}) - target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME}) - ament_target_dependencies(test_generic_pubsub${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) @@ -451,153 +332,79 @@ function(test_qos_event_for_rmw_implementation) ENV ${rmw_implementation_env_var} ) if(TARGET test_qos_event${target_suffix}) - target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_qos_event${target_suffix} - "rmw" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_qos_event_for_rmw_implementation) ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) if(TARGET test_qos_overriding_options) - target_link_libraries(test_qos_overriding_options - ${PROJECT_NAME} - ) + target_link_libraries(test_qos_overriding_options ${PROJECT_NAME}) endif() ament_add_gmock(test_qos_parameters test_qos_parameters.cpp) if(TARGET test_qos_parameters) - target_link_libraries(test_qos_parameters - ${PROJECT_NAME} - ) + target_link_libraries(test_qos_parameters ${PROJECT_NAME}) endif() ament_add_gtest(test_rate test_rate.cpp) if(TARGET test_rate) - ament_target_dependencies(test_rate - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_rate - ${PROJECT_NAME} - ) + target_link_libraries(test_rate ${PROJECT_NAME}) endif() ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp) if(TARGET test_serialized_message_allocator) - ament_target_dependencies(test_serialized_message_allocator - test_msgs - ) - target_link_libraries(test_serialized_message_allocator - ${PROJECT_NAME} - ) + target_link_libraries(test_serialized_message_allocator ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_serialized_message test_serialized_message.cpp) if(TARGET test_serialized_message) - ament_target_dependencies(test_serialized_message - test_msgs - ) - target_link_libraries(test_serialized_message - ${PROJECT_NAME} - ) + target_link_libraries(test_serialized_message ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) endif() ament_add_gtest(test_service test_service.cpp) if(TARGET test_service) - ament_target_dependencies(test_service - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_service ${PROJECT_NAME} mimick) + target_link_libraries(test_service ${PROJECT_NAME} mimick ${rcl_interfaces_TARGES} ${test_msgs_TARGETS}) endif() ament_add_gmock(test_service_introspection test_service_introspection.cpp) -if(TARGET test_service) - ament_target_dependencies(test_service_introspection - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick) +if(TARGET test_service_introspection) + target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick ${service_msgs_TARGETS} ${test_msgs_TARGETS}) endif() # Creating and destroying nodes is slow with Connext, so this needs larger timeout. ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) if(TARGET test_subscription) - ament_target_dependencies(test_subscription - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription ${PROJECT_NAME} mimick) + target_link_libraries(test_subscription ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp) if(TARGET test_subscription_publisher_count_api) - ament_target_dependencies(test_subscription_publisher_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) + target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_subscription_traits test_subscription_traits.cpp) if(TARGET test_subscription_traits) - ament_target_dependencies(test_subscription_traits - "rcl" - "test_msgs" - ) - target_link_libraries(test_subscription_traits ${PROJECT_NAME}) + target_link_libraries(test_subscription_traits ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_type_support test_type_support.cpp) if(TARGET test_type_support) - ament_target_dependencies(test_type_support - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_type_support ${PROJECT_NAME}) + target_link_libraries(test_type_support ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gmock(test_typesupport_helpers test_typesupport_helpers.cpp) if(TARGET test_typesupport_helpers) - target_link_libraries(test_typesupport_helpers ${PROJECT_NAME}) + target_link_libraries(test_typesupport_helpers ${PROJECT_NAME} rcpputils::rcpputils) endif() ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) - ament_target_dependencies(test_find_weak_nodes - "rcl" - ) target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) endif() ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp) -ament_target_dependencies(test_externally_defined_services - "rcl" - "test_msgs" -) -target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) +target_link_libraries(test_externally_defined_services ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) ament_add_gtest(test_duration test_duration.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_duration) - ament_target_dependencies(test_duration - "rcl") - target_link_libraries(test_duration ${PROJECT_NAME}) + target_link_libraries(test_duration ${PROJECT_NAME} rcl::rcl) endif() ament_add_gtest(test_logger test_logger.cpp) -target_link_libraries(test_logger ${PROJECT_NAME}) +target_link_libraries(test_logger ${PROJECT_NAME} rcutils::rcutils) ament_add_gmock(test_logging test_logging.cpp) -target_link_libraries(test_logging ${PROJECT_NAME}) +target_link_libraries(test_logging ${PROJECT_NAME} rcutils::rcutils) ament_add_gmock(test_context test_context.cpp) target_link_libraries(test_context ${PROJECT_NAME}) @@ -605,134 +412,108 @@ target_link_libraries(test_context ${PROJECT_NAME}) ament_add_gtest(test_time test_time.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_time) - ament_target_dependencies(test_time - "rcl") - target_link_libraries(test_time ${PROJECT_NAME}) + target_link_libraries(test_time ${PROJECT_NAME} rcl::rcl rcutils::rcutils) endif() ament_add_gtest(test_timer test_timer.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_timer) - ament_target_dependencies(test_timer - "rcl") - target_link_libraries(test_timer ${PROJECT_NAME} mimick) + target_link_libraries(test_timer ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_timers_manager test_timers_manager.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_timers_manager) - ament_target_dependencies(test_timers_manager - "rcl") - target_link_libraries(test_timers_manager ${PROJECT_NAME} mimick) + target_link_libraries(test_timers_manager ${PROJECT_NAME}) endif() ament_add_gtest(test_time_source test_time_source.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_time_source) - ament_target_dependencies(test_time_source - "rcl") - target_link_libraries(test_time_source ${PROJECT_NAME}) + target_link_libraries(test_time_source ${PROJECT_NAME} rcl::rcl) endif() ament_add_gtest(test_utilities test_utilities.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_utilities) - ament_target_dependencies(test_utilities - "rcl") - target_link_libraries(test_utilities ${PROJECT_NAME} mimick) + target_link_libraries(test_utilities ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_wait_for_message test_wait_for_message.cpp) if(TARGET test_wait_for_message) - ament_target_dependencies(test_wait_for_message - "test_msgs") - target_link_libraries(test_wait_for_message ${PROJECT_NAME}) + target_link_libraries(test_wait_for_message ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_logger_service test_logger_service.cpp) if(TARGET test_logger_service) - ament_target_dependencies(test_logger_service - "rcl_interfaces") - target_link_libraries(test_logger_service ${PROJECT_NAME}) + target_link_libraries(test_logger_service ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_interface_traits test_interface_traits.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_interface_traits) - ament_target_dependencies(test_interface_traits - "rcl") target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 ament_add_gtest( test_executors executors/test_executors.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) if(TARGET test_executors) - ament_target_dependencies(test_executors - "rcl" - "test_msgs") - target_link_libraries(test_executors ${PROJECT_NAME}) + target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_timer_cancel_behavior + executors/test_executors_timer_cancel_behavior.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_intraprocess + executors/test_executors_intraprocess.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_static_single_threaded_executor) - ament_target_dependencies(test_static_single_threaded_executor - "test_msgs") - target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick) + target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_multi_threaded_executor) - ament_target_dependencies(test_multi_threaded_executor - "rcl") target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() -ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) -if(TARGET test_static_executor_entities_collector) - ament_target_dependencies(test_static_executor_entities_collector - "rcl" - "test_msgs") - target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) -endif() - ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_entities_collector) - ament_target_dependencies(test_entities_collector - "rcl" - "test_msgs") - target_link_libraries(test_entities_collector ${PROJECT_NAME} mimick) + target_link_libraries(test_entities_collector ${PROJECT_NAME}) endif() ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_executor_notify_waitable) - ament_target_dependencies(test_executor_notify_waitable - "rcl" - "test_msgs") - target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick) + target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick rcpputils::rcpputils) endif() ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 5) if(TARGET test_events_executor) - ament_target_dependencies(test_events_executor - "test_msgs") - target_link_libraries(test_events_executor ${PROJECT_NAME}) + target_link_libraries(test_events_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_events_queue executors/test_events_queue.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_events_queue) - ament_target_dependencies(test_events_queue - "test_msgs") target_link_libraries(test_events_queue ${PROJECT_NAME}) endif() @@ -745,76 +526,60 @@ endif() ament_add_gtest(test_wait_set test_wait_set.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_wait_set) - ament_target_dependencies(test_wait_set "test_msgs") - target_link_libraries(test_wait_set ${PROJECT_NAME}) + target_link_libraries(test_wait_set ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_subscription_topic_statistics) - ament_target_dependencies(test_subscription_topic_statistics - "builtin_interfaces" - "libstatistics_collector" - "rcl_interfaces" - "rcutils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "statistics_msgs" - "test_msgs") target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME} - ${cpp_typesupport_target}) + libstatistics_collector::libstatistics_collector + ${statistics_msgs_TARGETS} + ${test_msgs_TARGETS} + ) endif() ament_add_gtest(test_subscription_options test_subscription_options.cpp) if(TARGET test_subscription_options) - ament_target_dependencies(test_subscription_options "rcl") target_link_libraries(test_subscription_options ${PROJECT_NAME}) endif() ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp) if(TARGET test_dynamic_storage) - ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs") - target_link_libraries(test_dynamic_storage ${PROJECT_NAME}) + target_link_libraries(test_dynamic_storage ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp) if(TARGET test_storage_policy_common) - ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs") - target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick) + target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp) if(TARGET test_static_storage) - ament_target_dependencies(test_static_storage "rcl" "test_msgs") - target_link_libraries(test_static_storage ${PROJECT_NAME}) + target_link_libraries(test_static_storage ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp) if(TARGET test_thread_safe_synchronization) - ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs") - target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME}) + target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_rosout_qos test_rosout_qos.cpp) if(TARGET test_rosout_qos) - ament_target_dependencies(test_rosout_qos "rcl") - target_link_libraries(test_rosout_qos ${PROJECT_NAME}) + target_link_libraries(test_rosout_qos ${PROJECT_NAME} rcl::rcl rmw::rmw) endif() ament_add_gtest(test_rosout_subscription test_rosout_subscription.cpp) if(TARGET test_rosout_subscription) - ament_target_dependencies(test_rosout_subscription "rcl") - target_link_libraries(test_rosout_subscription ${PROJECT_NAME}) + target_link_libraries(test_rosout_subscription ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_executor test_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_executor) - ament_target_dependencies(test_executor "rcl") target_link_libraries(test_executor ${PROJECT_NAME} mimick) endif() @@ -831,12 +596,7 @@ function(test_subscription_content_filter_for_rmw_implementation) TIMEOUT 120 ) if(TARGET test_subscription_content_filter${target_suffix}) - target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_subscription_content_filter${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) diff --git a/rclcpp/test/rclcpp/executors/executor_types.hpp b/rclcpp/test/rclcpp/executors/executor_types.hpp new file mode 100644 index 0000000000..0218a9b547 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/executor_types.hpp @@ -0,0 +1,70 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ + +#include + +#include +#include + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" + +using ExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; + +class ExecutorTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same()) { + return "SingleThreadedExecutor"; + } + + if (std::is_same()) { + return "MultiThreadedExecutor"; + } + + if (std::is_same()) { + return "StaticSingleThreadedExecutor"; + } + + if (std::is_same()) { + return "EventsExecutor"; + } + + return ""; + } +}; + +// StaticSingleThreadedExecutor is not included in these tests for now, due to: +// https://github.com/ros2/rclcpp/issues/1219 +using StandardExecutors = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; + +#endif // RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp index fbe83fcddc..13092b7067 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -43,11 +43,6 @@ class TestEventsExecutor : public ::testing::Test TEST_F(TestEventsExecutor, run_pub_sub) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); bool msg_received = false; @@ -65,7 +60,7 @@ TEST_F(TestEventsExecutor, run_pub_sub) executor.add_node(node); bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { + std::thread spinner([&spin_exited, &executor]() { executor.spin(); spin_exited = true; }); @@ -80,8 +75,6 @@ TEST_F(TestEventsExecutor, run_pub_sub) !spin_exited && (std::chrono::high_resolution_clock::now() - start < 1s)) { - auto time = std::chrono::high_resolution_clock::now() - start; - auto time_msec = std::chrono::duration_cast(time); std::this_thread::sleep_for(25ms); } @@ -95,11 +88,6 @@ TEST_F(TestEventsExecutor, run_pub_sub) TEST_F(TestEventsExecutor, run_clients_servers) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); bool request_received = false; @@ -119,7 +107,7 @@ TEST_F(TestEventsExecutor, run_clients_servers) executor.add_node(node); bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { + std::thread spinner([&spin_exited, &executor]() { executor.spin(); spin_exited = true; }); @@ -153,11 +141,6 @@ TEST_F(TestEventsExecutor, run_clients_servers) TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -190,11 +173,6 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) TEST_F(TestEventsExecutor, spin_once_max_duration_timer) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -226,11 +204,6 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timer) TEST_F(TestEventsExecutor, spin_some_max_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - { auto node = std::make_shared("node"); @@ -277,11 +250,6 @@ TEST_F(TestEventsExecutor, spin_some_max_duration) TEST_F(TestEventsExecutor, spin_some_zero_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); size_t t_runs = 0; @@ -303,11 +271,6 @@ TEST_F(TestEventsExecutor, spin_some_zero_duration) TEST_F(TestEventsExecutor, spin_all_max_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - { auto node = std::make_shared("node"); @@ -358,11 +321,6 @@ TEST_F(TestEventsExecutor, spin_all_max_duration) TEST_F(TestEventsExecutor, cancel_while_timers_running) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -388,7 +346,7 @@ TEST_F(TestEventsExecutor, cancel_while_timers_running) }); - std::thread spinner([&executor, this]() {executor.spin();}); + std::thread spinner([&executor]() {executor.spin();}); std::this_thread::sleep_for(10ms); // Call cancel while t1 callback is still being executed @@ -402,11 +360,6 @@ TEST_F(TestEventsExecutor, cancel_while_timers_running) TEST_F(TestEventsExecutor, cancel_while_timers_waiting) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); size_t t1_runs = 0; @@ -420,7 +373,7 @@ TEST_F(TestEventsExecutor, cancel_while_timers_waiting) executor.add_node(node); auto start = std::chrono::steady_clock::now(); - std::thread spinner([&executor, this]() {executor.spin();}); + std::thread spinner([&executor]() {executor.spin();}); std::this_thread::sleep_for(10ms); executor.cancel(); @@ -435,11 +388,6 @@ TEST_F(TestEventsExecutor, destroy_entities) // This test fails on Windows! We skip it for now GTEST_SKIP(); - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - // Create a publisher node and start publishing messages auto node_pub = std::make_shared("node_pub"); auto publisher = node_pub->create_publisher("topic", rclcpp::QoS(10)); @@ -447,7 +395,7 @@ TEST_F(TestEventsExecutor, destroy_entities) 2ms, [&]() {publisher->publish(std::make_unique());}); EventsExecutor executor_pub; executor_pub.add_node(node_pub); - std::thread spinner([&executor_pub, this]() {executor_pub.spin();}); + std::thread spinner([&executor_pub]() {executor_pub.spin();}); // Create a node with two different subscriptions to the topic auto node_sub = std::make_shared("node_sub"); @@ -485,11 +433,6 @@ std::string * g_sub_log_msg; std::promise * g_log_msgs_promise; TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); rcutils_logging_output_handler_t original_output_handler = rcutils_logging_get_output_handler(); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 38b6ddf870..3434f473c6 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -15,17 +15,19 @@ /** * This test checks all implementations of rclcpp::executor to check they pass they basic API * tests. Anything specific to any executor in particular should go in a separate test file. - * */ + #include #include +#include #include #include #include #include #include #include +#include #include "rcl/error_handling.h" #include "rcl/time.h" @@ -34,27 +36,22 @@ #include "rclcpp/duration.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" #include "test_msgs/msg/empty.hpp" +#include "./executor_types.hpp" + using namespace std::chrono_literals; template class TestExecutors : public ::testing::Test { public: - static void SetUpTestCase() + void SetUp() { rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - void SetUp() - { const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); std::stringstream test_name; test_name << test_info->test_case_name() << "_" << test_info->name(); @@ -75,6 +72,8 @@ class TestExecutors : public ::testing::Test publisher.reset(); subscription.reset(); node.reset(); + + rclcpp::shutdown(); } rclcpp::Node::SharedPtr node; @@ -83,70 +82,17 @@ class TestExecutors : public ::testing::Test int callback_count; }; -// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see: -// https://github.com/ros2/rclcpp/issues/1219 for tracking template class TestExecutorsStable : public TestExecutors {}; -using ExecutorTypes = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor, - rclcpp::experimental::executors::EventsExecutor>; - -class ExecutorTypeNames -{ -public: - template - static std::string GetName(int idx) - { - (void)idx; - if (std::is_same()) { - return "SingleThreadedExecutor"; - } - - if (std::is_same()) { - return "MultiThreadedExecutor"; - } - - if (std::is_same()) { - return "StaticSingleThreadedExecutor"; - } - - if (std::is_same()) { - return "EventsExecutor"; - } - - return ""; - } -}; - -// TYPED_TEST_SUITE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency -// is updated. TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); -// StaticSingleThreadedExecutor is not included in these tests for now, due to: -// https://github.com/ros2/rclcpp/issues/1219 -using StandardExecutors = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::experimental::executors::EventsExecutor>; TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing TYPED_TEST(TestExecutors, detachOnDestruction) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - { ExecutorType executor; executor.add_node(this->node); @@ -158,19 +104,8 @@ TYPED_TEST(TestExecutors, detachOnDestruction) } // Make sure that the executor can automatically remove expired nodes correctly -// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see: -// https://github.com/ros2/rclcpp/issues/1231 -TYPED_TEST(TestExecutorsStable, addTemporaryNode) -{ +TYPED_TEST(TestExecutors, addTemporaryNode) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; { @@ -191,14 +126,6 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) TYPED_TEST(TestExecutors, emptyExecutor) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); std::this_thread::sleep_for(50ms); @@ -210,14 +137,6 @@ TYPED_TEST(TestExecutors, emptyExecutor) TYPED_TEST(TestExecutors, addNodeTwoExecutors) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor1; ExecutorType executor2; EXPECT_NO_THROW(executor1.add_node(this->node)); @@ -229,14 +148,6 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) TYPED_TEST(TestExecutors, spinWithTimer) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; bool timer_completed = false; @@ -260,25 +171,20 @@ TYPED_TEST(TestExecutors, spinWithTimer) TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; - executor.add_node(this->node); - bool timer_completed = false; - auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + std::atomic_bool timer_completed = false; + auto timer = this->node->create_wall_timer( + 1ms, [&]() { + timer_completed.store(true); + }); + executor.add_node(this->node); std::thread spinner([&]() {executor.spin();}); - // Sleep for a short time to verify executor.spin() is going, and didn't throw. + // Sleep for a short time to verify executor.spin() is going, and didn't throw. auto start = std::chrono::steady_clock::now(); - while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) { + while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) { std::this_thread::sleep_for(1ms); } @@ -295,14 +201,6 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -326,14 +224,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -358,14 +248,6 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -413,14 +295,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -463,21 +337,33 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() = default; void - add_to_wait_set(rcl_wait_set_t * wait_set) override + add_to_wait_set(rcl_wait_set_t & wait_set) override { - rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + if (trigger_count_ > 0) { + // Keep the gc triggered until the trigger count is reduced back to zero. + // This is necessary if trigger() results in the wait set waking, but not + // executing this waitable, in which case it needs to be re-triggered. + gc_.trigger(); + } + rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); } void trigger() { + trigger_count_++; gc_.trigger(); } bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t & wait_set) override { - (void)wait_set; - return true; + for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) { + auto rcl_guard_condition = wait_set.guard_conditions[i]; + if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) { + return true; + } + } + return false; } std::shared_ptr @@ -494,11 +380,24 @@ class TestWaitable : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr &) override { - (void) data; + trigger_count_--; count_++; - std::this_thread::sleep_for(3ms); + if (nullptr != on_execute_callback_) { + on_execute_callback_(); + } else { + // TODO(wjwwood): I don't know why this was here, but probably it should + // not be there, or test cases where that is important should use the + // on_execute_callback? + std::this_thread::sleep_for(3ms); + } + } + + void + set_on_execute_callback(std::function on_execute_callback) + { + on_execute_callback_ = on_execute_callback; } void @@ -520,27 +419,21 @@ class TestWaitable : public rclcpp::Waitable get_number_of_ready_guard_conditions() override {return 1;} size_t - get_count() + get_count() const { return count_; } private: - size_t count_ = 0; + std::atomic trigger_count_ = 0; + std::atomic count_ = 0; rclcpp::GuardCondition gc_; + std::function on_execute_callback_ = nullptr; }; TYPED_TEST(TestExecutors, spinAll) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); auto my_waitable = std::make_shared(); @@ -580,67 +473,188 @@ TYPED_TEST(TestExecutors, spinAll) spinner.join(); } -TYPED_TEST(TestExecutors, spinSome) +// Helper function to convert chrono durations into a scalar that GoogleTest +// can more easily compare and print. +template +auto +to_nanoseconds_helper(DurationT duration) +{ + return std::chrono::duration_cast(duration).count(); +} + +// The purpose of this test is to check that the ExecutorT.spin_some() method: +// - works nominally (it can execute entities) +// - it can execute multiple items at once +// - it does not wait for work to be available before returning +TYPED_TEST(TestExecutors, spin_some) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Check that spin_some() returns quickly when there is no work to be done. + // This can be a false positive if there is somehow some work for the executor + // to do that has not been considered, but the isolated callback group should + // avoid that. + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some with some non-trival "max_duration" and check that it does not + // take anywhere near that long to execute. + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + } + + // Check that having one thing ready gets executed by spin_some(). auto waitable_interfaces = this->node->get_node_waitables_interface(); - auto my_waitable = std::make_shared(); - waitable_interfaces->add_waitable(my_waitable, nullptr); - executor.add_node(this->node); + auto my_waitable1 = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); - // Long timeout, doesn't block test from finishing because spin_some should exit after the - // first one completes. - bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { - executor.spin_some(1s); - executor.remove_node(this->node, true); - spin_exited = true; - }); + my_waitable1->trigger(); - // Do some work until sufficient calls to the waitable occur, but keep going until either - // count becomes too large, spin exits, or the 1 second timeout completes. - auto start = std::chrono::steady_clock::now(); - while ( - my_waitable->get_count() <= 1 && - !spin_exited && - (std::chrono::steady_clock::now() - start < 1s)) - { - my_waitable->trigger(); - this->publisher->publish(test_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); + // The long duration should not matter, as executing the waitable is + // non-blocking, and spin_some() should exit after completing the available + // work. + auto start = std::chrono::steady_clock::now(); + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have very " + << "little to do and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + EXPECT_EQ(my_waitable1->get_count(), 1u) + << "spin_some() failed to execute a waitable that was triggered"; } - // The count of "execute" depends on whether the executor starts spinning before (1) or after (0) - // the first iteration of the while loop - EXPECT_LE(1u, my_waitable->get_count()); - waitable_interfaces->remove_waitable(my_waitable, nullptr); - EXPECT_TRUE(spin_exited); - // Cancel if it hasn't exited already. - executor.cancel(); - spinner.join(); + // Check that multiple things being ready are executed by spin_some(). + auto my_waitable2 = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + const size_t original_my_waitable1_count = my_waitable1->get_count(); + my_waitable1->trigger(); + my_waitable2->trigger(); + + // The long duration should not matter, as executing the waitable is + // non-blocking, and spin_some() should exit after completing the available + // work. + auto start = std::chrono::steady_clock::now(); + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have very " + << "little to do and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + EXPECT_EQ(my_waitable1->get_count(), original_my_waitable1_count + 1) + << "spin_some() failed to execute a waitable that was triggered"; + EXPECT_EQ(my_waitable2->get_count(), 1u) + << "spin_some() failed to execute a waitable that was triggered"; + } } -// Check spin_node_until_future_complete with node base pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) +// The purpose of this test is to check that the ExecutorT.spin_some() method: +// - does not continue executing after max_duration has elapsed +TYPED_TEST(TestExecutors, spin_some_max_duration) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor + + // TODO(wjwwood): The `StaticSingleThreadedExecutor` + // do not properly implement max_duration (it seems), so disable this test + // for them in the meantime. + // see: https://github.com/ros2/rclcpp/issues/2462 if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + std::is_same()) { GTEST_SKIP(); } + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Set up a situation with two waitables that take time to execute, such that + // the time it takes to execute two waitables far exceeds the max_duration + // given to spin_some(), which should result in spin_some() starting to + // execute one of them, have the max duration elapse, finish executing one + // of them, then returning before starting on the second. + constexpr auto max_duration = 100ms; // relatively short because we expect to exceed it + constexpr auto waitable_callback_duration = max_duration * 2; + auto long_running_callback = [&waitable_callback_duration]() { + std::this_thread::sleep_for(waitable_callback_duration); + }; + + auto waitable_interfaces = this->node->get_node_waitables_interface(); + + auto my_waitable1 = std::make_shared(); + my_waitable1->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + + auto my_waitable2 = std::make_shared(); + my_waitable2->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + + my_waitable1->trigger(); + my_waitable2->trigger(); + + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some and check that it does not take longer than two of waitable_callback_duration, + // nor significantly less than a single waitable_callback_duration. + executor.spin_some(max_duration); + auto spin_some_run_time = std::chrono::steady_clock::now() - start; + EXPECT_GT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration / 2)) + << "spin_some() took less than half the expected time to execute a single " + << "waitable, which implies it did not actually execute one when it was " + << "expected to"; + EXPECT_LT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration * 2)) + << "spin_some() took longer than expected to execute by a significant margin, but " + << "this could be a false positive on a very slow computer"; + + // check that exactly one of the waitables were executed (do not depend on a specific order) + size_t number_of_waitables_executed = my_waitable1->get_count() + my_waitable2->get_count(); + EXPECT_EQ(number_of_waitables_executed, 1u) + << "expected exactly one of the two waitables to be executed, but " + << "my_waitable1->get_count(): " << my_waitable1->get_count() << " and " + << "my_waitable2->get_count(): " << my_waitable2->get_count(); +} + +// Check spin_node_until_future_complete with node base pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) +{ + using ExecutorType = TypeParam; ExecutorType executor; std::promise promise; @@ -657,14 +671,6 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; std::promise promise; @@ -681,14 +687,6 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -729,6 +727,78 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) spinner.join(); } +// This test verifies that the add_node operation is robust wrt race conditions. +// It's mostly meant to prevent regressions in the events-executor, but the operation should be +// thread-safe in all executor implementations. +// The initial implementation of the events-executor contained a bug where the executor +// would end up in an inconsistent state and stop processing interrupt/shutdown notifications. +// Manually adding a node to the executor results in a) producing a notify waitable event +// and b) refreshing the executor collections. +// The inconsistent state would happen if the event was processed before the collections were +// finished to be refreshed: the executor would pick up the event but be unable to process it. +// This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional +// notify waitable events to be pushed. +// The behavior is observable only under heavy load, so this test spawns several worker +// threads. Due to the nature of the bug, this test may still succeed even if the +// bug is present. However repeated runs will show its flakiness nature and indicate +// an eventual regression. +TYPED_TEST(TestExecutors, testRaceConditionAddNode) +{ + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + // Spawn some threads to do some heavy work + std::atomic should_cancel = false; + std::vector stress_threads; + for (size_t i = 0; i < 5 * std::thread::hardware_concurrency(); i++) { + stress_threads.emplace_back( + [&should_cancel, i]() { + // This is just some arbitrary heavy work + volatile size_t total = 0; + for (size_t k = 0; k < 549528914167; k++) { + if (should_cancel) { + break; + } + total += k * (i + 42); + (void)total; + } + }); + } + + // Create an executor + auto executor = std::make_shared(); + // Start spinning + auto executor_thread = std::thread( + [executor]() { + executor->spin(); + }); + // Add a node to the executor + executor->add_node(this->node); + + // Cancel the executor (make sure that it's already spinning first) + while (!executor->is_spinning() && rclcpp::ok()) { + continue; + } + executor->cancel(); + + // Try to join the thread after cancelling the executor + // This is the "test". We want to make sure that we can still cancel the executor + // regardless of the presence of race conditions + executor_thread.join(); + + // The test is now completed: we can join the stress threads + should_cancel = true; + for (auto & t : stress_threads) { + t.join(); + } +} + // Check spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { @@ -769,106 +839,3 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) rclcpp::shutdown(); } - -template -class TestIntraprocessExecutors : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); - std::stringstream test_name; - test_name << test_info->test_case_name() << "_" << test_info->name(); - node = std::make_shared("node", test_name.str()); - - callback_count = 0; - - const std::string topic_name = std::string("topic_") + test_name.str(); - - rclcpp::PublisherOptions po; - po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); - - auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { - this->callback_count.fetch_add(1); - }; - - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - subscription = - node->create_subscription( - topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); - } - - void TearDown() - { - publisher.reset(); - subscription.reset(); - node.reset(); - } - - const size_t kNumMessages = 100; - - rclcpp::Node::SharedPtr node; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr subscription; - std::atomic_int callback_count; -}; - -TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); - -TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { - // This tests that executors will continue to service intraprocess subscriptions in the case - // that publishers aren't continuing to publish. - // This was previously broken in that intraprocess guard conditions were only triggered on - // publish and the test was added to prevent future regressions. - const size_t kNumMessages = 100; - - using ExecutorType = TypeParam; - ExecutorType executor; - executor.add_node(this->node); - - EXPECT_EQ(0, this->callback_count.load()); - this->publisher->publish(test_msgs::msg::Empty()); - - // Wait for up to 5 seconds for the first message to come available. - const std::chrono::milliseconds sleep_per_loop(10); - int loops = 0; - while (1u != this->callback_count.load() && loops < 500) { - rclcpp::sleep_for(sleep_per_loop); - executor.spin_some(); - loops++; - } - EXPECT_EQ(1u, this->callback_count.load()); - - // reset counter - this->callback_count.store(0); - - for (size_t ii = 0; ii < kNumMessages; ++ii) { - this->publisher->publish(test_msgs::msg::Empty()); - } - - // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. - loops = 0; - auto timer = this->node->create_wall_timer( - std::chrono::milliseconds(10), [this, &executor, &loops, &kNumMessages]() { - loops++; - if (kNumMessages == this->callback_count.load() || - loops == 500) - { - executor.cancel(); - } - }); - executor.spin(); - EXPECT_EQ(kNumMessages, this->callback_count.load()); -} diff --git a/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp new file mode 100644 index 0000000000..af5f7e432e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "./executor_types.hpp" + +template +class TestIntraprocessExecutors : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + + callback_count = 0u; + + const std::string topic_name = std::string("topic_") + test_name.str(); + + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); + + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { + this->callback_count.fetch_add(1u); + }; + + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + subscription = + node->create_subscription( + topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); + } + + void TearDown() + { + publisher.reset(); + subscription.reset(); + node.reset(); + } + + const size_t kNumMessages = 100; + + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + std::atomic_size_t callback_count; +}; + +TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { + // This tests that executors will continue to service intraprocess subscriptions in the case + // that publishers aren't continuing to publish. + // This was previously broken in that intraprocess guard conditions were only triggered on + // publish and the test was added to prevent future regressions. + static constexpr size_t kNumMessages = 100; + + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + EXPECT_EQ(0u, this->callback_count.load()); + this->publisher->publish(test_msgs::msg::Empty()); + + // Wait for up to 5 seconds for the first message to come available. + const std::chrono::milliseconds sleep_per_loop(10); + int loops = 0; + while (1u != this->callback_count.load() && loops < 500) { + rclcpp::sleep_for(sleep_per_loop); + executor.spin_some(); + loops++; + } + EXPECT_EQ(1u, this->callback_count.load()); + + // reset counter + this->callback_count.store(0u); + + for (size_t ii = 0; ii < kNumMessages; ++ii) { + this->publisher->publish(test_msgs::msg::Empty()); + } + + // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. + loops = 0; + auto timer = this->node->create_wall_timer( + std::chrono::milliseconds(10), [this, &executor, &loops]() { + loops++; + if (kNumMessages == this->callback_count.load() || loops == 500) { + executor.cancel(); + } + }); + executor.spin(); + EXPECT_EQ(kNumMessages, this->callback_count.load()); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp new file mode 100644 index 0000000000..7fd1676c5d --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -0,0 +1,430 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/utilities.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +class TimerNode : public rclcpp::Node +{ +public: + explicit TimerNode(std::string subname) + : Node("timer_node", subname) + { + } + + void CreateTimer1() + { + timer1_ = rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer1Callback, this)); + } + + void CreateTimer2() + { + timer2_ = rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer2Callback, this)); + } + + int GetTimer1Cnt() {return cnt1_;} + int GetTimer2Cnt() {return cnt2_;} + + void ResetTimer1() + { + timer1_->reset(); + } + + void ResetTimer2() + { + timer2_->reset(); + } + + void CancelTimer1() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!"); + timer1_->cancel(); + } + + void CancelTimer2() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!"); + timer2_->cancel(); + } + +private: + void Timer1Callback() + { + cnt1_++; + RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_); + } + + void Timer2Callback() + { + cnt2_++; + RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_); + } + + rclcpp::TimerBase::SharedPtr timer1_; + rclcpp::TimerBase::SharedPtr timer2_; + int cnt1_ = 0; + int cnt2_ = 0; +}; + +// Sets up a separate thread to publish /clock messages. +// Clock rate relative to real clock is controlled by realtime_update_rate. +// This is set conservatively slow to ensure unit tests are reliable on Windows +// environments, where timing performance is subpar. +// +// Use `sleep_for` in tests to advance the clock. Clock should run and be published +// in separate thread continuously to ensure correct behavior in node under test. +class ClockPublisher : public rclcpp::Node +{ +public: + explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) + : Node("clock_publisher"), + ros_update_duration_(0, 0), + realtime_clock_step_(0, 0), + rostime_(0, 0) + { + clock_publisher_ = this->create_publisher("clock", 10); + realtime_clock_step_ = + rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate); + ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step); + + timer_thread_ = std::thread(&ClockPublisher::RunTimer, this); + } + + ~ClockPublisher() + { + running_ = false; + if (timer_thread_.joinable()) { + timer_thread_.join(); + } + } + + void sleep_for(rclcpp::Duration duration) + { + rclcpp::Time start_time(0, 0, RCL_ROS_TIME); + { + const std::lock_guard lock(mutex_); + start_time = rostime_; + } + rclcpp::Time current_time = start_time; + + while (true) { + { + const std::lock_guard lock(mutex_); + current_time = rostime_; + } + if ((current_time - start_time) >= duration) { + return; + } + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + rostime_ += ros_update_duration_; + } + } + +private: + void RunTimer() + { + while (running_) { + PublishClock(); + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + } + } + + void PublishClock() + { + const std::lock_guard lock(mutex_); + auto message = rosgraph_msgs::msg::Clock(); + message.clock = rostime_; + clock_publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr clock_publisher_; + + rclcpp::Duration ros_update_duration_; + rclcpp::Duration realtime_clock_step_; + // Rostime must be guarded by a mutex, since accessible in running thread + // as well as sleep_for + rclcpp::Time rostime_; + std::mutex mutex_; + std::thread timer_thread_; + std::atomic running_ = true; +}; + + +template +class TestTimerCancelBehavior : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared(test_name.str()); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", true)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + + // Check if the clock type is simulation time + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + + // Create timers + this->node->CreateTimer1(); + this->node->CreateTimer2(); + + // Run standalone thread to publish clock time + sim_clock_node = std::make_shared(); + + // Spin the executor in a standalone thread + executor.add_node(this->node); + standalone_thread = std::thread( + [this]() { + executor.spin(); + }); + } + + void TearDown() + { + node.reset(); + + // Clean up thread object + if (standalone_thread.joinable()) { + standalone_thread.join(); + } + } + + std::shared_ptr node; + std::shared_ptr sim_clock_node; + rclcpp::SyncParametersClient::SharedPtr param_client; + std::thread standalone_thread; + T executor; +}; + +using MainExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor>; + +// TODO(@fujitatomoya): this test excludes EventExecutor because it does not +// support simulation time used for this test to relax the racy condition. +// See more details for https://github.com/ros2/rclcpp/issues/2457. +TYPED_TEST_SUITE(TestTimerCancelBehavior, MainExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t2 has significantly more calls + EXPECT_LT(t1_runs + 50, t2_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t1 has significantly more calls + EXPECT_LT(t2_runs + 50, t1_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); + + EXPECT_LT(t1_runs_initial + 50, t2_runs_initial); + // Check that t2 has significantly more calls, and keeps getting called. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T2 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); + + EXPECT_LT(t2_runs_initial + 50, t1_runs_initial); + // Check that t1 has significantly more calls, and keeps getting called. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t2_runs_initial, t2_runs_intermediate); + EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t1_runs_initial, t1_runs_intermediate); + EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 1ea91029f4..1c8c5b3abe 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -230,9 +230,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return true;} + bool is_ready(const rcl_wait_set_t &) override {return true;} std::shared_ptr take_data() override @@ -240,10 +240,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } void - execute(std::shared_ptr & data) override - { - (void) data; - } + execute(const std::shared_ptr &) override {} }; TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { @@ -513,11 +510,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - EXPECT_THROW( - entities_collector_->add_to_wait_set(nullptr), - std::invalid_argument); - rcl_reset_error(); - EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); } diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 5ca6c1c25a..1a0f3f88c4 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -56,7 +56,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.add_callback_group(cb_group, node->get_node_base_interface(), true), - std::runtime_error("error not set")); + std::runtime_error("Failed to trigger guard condition on callback group add: error not set")); } } @@ -69,7 +69,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.add_node(node), - std::runtime_error("error not set")); + std::runtime_error("Failed to trigger guard condition on node add: error not set")); } } @@ -86,7 +86,8 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_callback_group(cb_group, true), - std::runtime_error("error not set")); + std::runtime_error( + "Failed to trigger guard condition on callback group remove: error not set")); } } @@ -99,7 +100,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_node(node, true), - std::runtime_error("Node needs to be associated with this executor.")); + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); } } @@ -114,7 +115,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_node(node, true), - std::runtime_error("error not set")); + std::runtime_error("Failed to trigger guard condition on node remove: error not set")); } } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 70ccb5622d..3882d2b71c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -129,6 +129,9 @@ TEST_F(TestNodeGraph, construct_from_node) EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic")); EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_clients("not_a_service")); + EXPECT_EQ(0u, node_graph()->count_services("not_a_service")); + EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition()); // get_graph_event is non-const @@ -534,6 +537,22 @@ TEST_F(TestNodeGraph, count_subscribers_rcl_error) std::runtime_error("could not count subscribers: error not set")); } +TEST_F(TestNodeGraph, count_clients_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_clients, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_clients("service"), + std::runtime_error("could not count clients: error not set")); +} + +TEST_F(TestNodeGraph, count_services_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_services, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_services("service"), + std::runtime_error("could not count services: error not set")); +} + TEST_F(TestNodeGraph, notify_shutdown) { EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown()); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 58bf010767..d262c67a9a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -77,9 +77,9 @@ TEST_F(TestNodeParameters, list_parameters) std::vector prefixes; const auto list_result = node_parameters->list_parameters(prefixes, 1u); - // Currently the only default parameter is 'use_sim_time', but that may change. + // Currently the default parameters are 'use_sim_time' and 'start_type_description_service' size_t number_of_parameters = list_result.names.size(); - EXPECT_GE(1u, number_of_parameters); + EXPECT_GE(2u, number_of_parameters); const std::string parameter_name = "new_parameter"; const rclcpp::ParameterValue value(true); @@ -95,15 +95,15 @@ TEST_F(TestNodeParameters, list_parameters) std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), list_result2.names.end()); - // Check prefixes + // Check prefixes and the depth relative to the given prefixes const std::string parameter_name2 = "prefix.new_parameter"; const rclcpp::ParameterValue value2(true); const rcl_interfaces::msg::ParameterDescriptor descriptor2; const auto added_parameter_value2 = node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false); - EXPECT_EQ(value.get(), added_parameter_value.get()); + EXPECT_EQ(value2.get(), added_parameter_value2.get()); prefixes = {"prefix"}; - auto list_result3 = node_parameters->list_parameters(prefixes, 2u); + auto list_result3 = node_parameters->list_parameters(prefixes, 1u); EXPECT_EQ(1u, list_result3.names.size()); EXPECT_NE( std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2), @@ -116,6 +116,13 @@ TEST_F(TestNodeParameters, list_parameters) EXPECT_NE( std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name), list_result4.names.end()); + + // Return all parameters when the depth = 0 + auto list_result5 = node_parameters->list_parameters(prefixes, 0u); + EXPECT_EQ(1u, list_result5.names.size()); + EXPECT_NE( + std::find(list_result5.names.begin(), list_result5.names.end(), parameter_name), + list_result5.names.end()); } TEST_F(TestNodeParameters, parameter_overrides) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index ea55a1aac2..7b00fea972 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -92,7 +92,7 @@ TEST_F(TestNodeService, add_service) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group_in_different_node), - std::runtime_error("Cannot create service, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("service")); } TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) @@ -119,7 +119,7 @@ TEST_F(TestNodeService, add_client) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group_in_different_node), - std::runtime_error("Cannot create client, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("client")); } TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index d038a4b44d..dc92dee610 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -75,7 +75,7 @@ TEST_F(TestNodeTimers, add_timer) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group_in_different_node), - std::runtime_error("Cannot create timer, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("timer")); } TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp new file mode 100644 index 0000000000..1a4603528a --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp @@ -0,0 +1,61 @@ +// 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 + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" + +class TestNodeTypeDescriptions : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTypeDescriptions, interface_created) +{ + rclcpp::Node node{"node", "ns"}; + ASSERT_NE(nullptr, node.get_node_type_descriptions_interface()); +} + +TEST_F(TestNodeTypeDescriptions, disabled_no_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", false); + rclcpp::Node node{"node", "ns", node_options}; + + auto services = node.get_node_graph_interface()->get_service_names_and_types_by_node( + "node", "/ns"); + EXPECT_TRUE(services.find("/ns/node/get_type_description") == services.end()); +} + +TEST_F(TestNodeTypeDescriptions, enabled_creates_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", true); + rclcpp::Node node{"node", "ns", node_options}; + + auto services = node.get_node_graph_interface()->get_service_names_and_types_by_node( + "node", "/ns"); + + EXPECT_TRUE(services.find("/ns/node/get_type_description") != services.end()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index a1f35c0cdc..aa34a71af4 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -28,8 +28,8 @@ class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return false;} + void add_to_wait_set(rcl_wait_set_t &) override {} + bool is_ready(const rcl_wait_set_t &) override {return false;} std::shared_ptr take_data() override @@ -37,10 +37,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override - { - (void) data; - } + void execute(const std::shared_ptr &) override {} }; class TestNodeWaitables : public ::testing::Test @@ -78,7 +75,7 @@ TEST_F(TestNodeWaitables, add_remove_waitable) node_waitables->add_waitable(waitable, callback_group1)); RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group2), - std::runtime_error("Cannot create waitable, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("waitable")); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 2adf907bc5..d25b3490c2 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -39,14 +39,14 @@ static bool test_waitable_result = false; class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t &) override { if (!test_waitable_result) { throw std::runtime_error("TestWaitable add_to_wait_set failed"); } } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t &) override { return test_waitable_result; } @@ -57,10 +57,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override - { - (void) data; - } + void execute(const std::shared_ptr &) override {} }; static bool test_waitable_result2 = false; @@ -82,12 +79,12 @@ class TestWaitable2 : public rclcpp::Waitable EXPECT_EQ(rcl_event_fini(&pub_event_), RCL_RET_OK); } - void add_to_wait_set(rcl_wait_set_t * wait_set) override + void add_to_wait_set(rcl_wait_set_t & wait_set) override { - EXPECT_EQ(rcl_wait_set_add_event(wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK); + EXPECT_EQ(rcl_wait_set_add_event(&wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK); } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t &) override { return test_waitable_result2; } @@ -98,7 +95,7 @@ class TestWaitable2 : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { (void) data; } @@ -200,9 +197,9 @@ class TestAllocatorMemoryStrategy : public ::testing::Test std::shared_ptr create_node_with_service(const std::string & name) { - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; auto node_with_service = create_node_with_disabled_callback_groups(name); auto callback_group = @@ -949,9 +946,9 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = node->create_service( "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group); diff --git a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp index b68bc96ab6..69198adf59 100644 --- a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp @@ -56,18 +56,28 @@ TEST_F(TestMessagePoolMemoryStrategy, borrow_too_many) { // Size is 1, borrowing second time should fail RCLCPP_EXPECT_THROW_EQ( message_memory_strategy_->borrow_message(), - std::runtime_error("Tried to access message that was still in use! Abort.")); + std::runtime_error("No more free slots in the pool")); EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); } -TEST_F(TestMessagePoolMemoryStrategy, return_unrecognized) { - auto message = message_memory_strategy_->borrow_message(); - ASSERT_NE(nullptr, message); +TEST_F(TestMessagePoolMemoryStrategy, borrow_hold_reference) { + { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); - auto unrecognized = std::make_shared(); - // Unrecognized does not belong to pool - RCLCPP_EXPECT_THROW_EQ( - message_memory_strategy_->return_message(unrecognized), - std::runtime_error("Unrecognized message ptr in return_message.")); - EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); + // Return it. + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); + + // But we are still holding the reference, so we expect that there is still no room in the pool. + RCLCPP_EXPECT_THROW_EQ( + message_memory_strategy_->borrow_message(), + std::runtime_error("No more free slots in the pool")); + } + + // Now that we've dropped the reference (left the scope), we expect to be able to borrow again. + + auto message2 = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message2); + + EXPECT_NO_THROW(message_memory_strategy_->return_message(message2)); } diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 49131638db..fefec6c8fa 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -99,13 +99,6 @@ TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, Execu TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; @@ -155,13 +148,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; @@ -193,13 +179,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); @@ -220,30 +199,32 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } + + auto count_callback_groups_in_node = [](auto node) { + size_t num = 0; + node->get_node_base_interface()->for_each_callback_group( + [&num](auto) { + num++; + }); + return num; + }; ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); executor.add_node(node->get_node_base_interface()); - ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); - std::atomic_int timer_count {0}; + ASSERT_EQ(executor.get_all_callback_groups().size(), count_callback_groups_in_node(node)); + std::atomic_size_t timer_count {0}; auto timer_callback = [&executor, &timer_count]() { - if (timer_count > 0) { - ASSERT_EQ(executor.get_all_callback_groups().size(), 3u); + auto cur_timer_count = timer_count++; + printf("in timer_callback(%zu)\n", cur_timer_count); + if (cur_timer_count > 0) { executor.cancel(); } - timer_count++; }; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( - 2s, timer_callback, cb_grp); + 1s, timer_callback, cb_grp); rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); auto timer2_callback = []() {}; @@ -255,6 +236,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( 2s, timer3_callback, cb_grp3); executor.spin(); + ASSERT_GT(timer_count.load(), 0u); } /* @@ -263,13 +245,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); @@ -307,13 +282,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_executors) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType timer_executor; ExecutorType sub_executor; @@ -355,13 +323,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receive_message) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); @@ -428,13 +389,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receiv TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_spin) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); @@ -481,13 +435,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_sp TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_group) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp index 6a70b8b5da..b6e49097c3 100644 --- a/rclcpp/test/rclcpp/test_any_service_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -51,8 +51,10 @@ TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) { TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { int callback_calls = 0; - auto callback = [&callback_calls](const std::shared_ptr, - std::shared_ptr) { + auto callback = [&callback_calls]( + const std::shared_ptr, + std::shared_ptr) + { callback_calls++; }; @@ -65,10 +67,11 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { int callback_with_header_calls = 0; - auto callback_with_header = - [&callback_with_header_calls](const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr) { + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) + { callback_with_header_calls++; }; @@ -80,9 +83,9 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) { int callback_with_header_calls = 0; - auto callback_with_header = - [&callback_with_header_calls](const std::shared_ptr, - const std::shared_ptr) { + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, const std::shared_ptr) + { callback_with_header_calls++; }; @@ -94,10 +97,10 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) { TEST_F(TestAnyServiceCallback, set_and_dispatch_defered_with_service_handle) { int callback_with_header_calls = 0; - auto callback_with_header = - [&callback_with_header_calls](std::shared_ptr>, - const std::shared_ptr, - const std::shared_ptr) + auto callback_with_header = [&callback_with_header_calls]( + std::shared_ptr>, + const std::shared_ptr, + const std::shared_ptr) { callback_with_header_calls++; }; diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 45fe091f07..a98271e931 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -19,8 +19,6 @@ #include #include -// TODO(aprotyas): Figure out better way to suppress deprecation warnings. -#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1 #include "rclcpp/any_subscription_callback.hpp" #include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.h" diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 9070e1caa9..5c7e5046ab 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -24,12 +24,9 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/srv/empty.hpp" -using namespace std::chrono_literals; - class TestClient : public ::testing::Test { protected: @@ -219,385 +216,3 @@ TEST_F(TestClientSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } - -class TestClientWithServer : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - node = std::make_shared("node", "ns"); - - auto callback = []( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; - - service = node->create_service(service_name, std::move(callback)); - } - - ::testing::AssertionResult SendEmptyRequestAndWait( - std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) - { - using SharedFuture = rclcpp::Client::SharedFuture; - - auto client = node->create_client(service_name); - if (!client->wait_for_service()) { - return ::testing::AssertionFailure() << "Waiting for service failed"; - } - - auto request = std::make_shared(); - bool received_response = false; - ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); - auto callback = [&received_response, &request_result](SharedFuture future_response) { - if (nullptr == future_response.get()) { - request_result = ::testing::AssertionFailure() << "Future response was null"; - } - received_response = true; - }; - - auto req_id = client->async_send_request(request, std::move(callback)); - - auto start = std::chrono::steady_clock::now(); - while (!received_response && - (std::chrono::steady_clock::now() - start) < timeout) - { - rclcpp::spin_some(node); - } - - if (!received_response) { - return ::testing::AssertionFailure() << "Waiting for response timed out"; - } - if (client->remove_pending_request(req_id)) { - return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; - } - - return request_result; - } - - std::shared_ptr node; - std::shared_ptr> service; - const std::string service_name{"empty_service"}; -}; - -TEST_F(TestClientWithServer, async_send_request) { - EXPECT_TRUE(SendEmptyRequestAndWait()); -} - -TEST_F(TestClientWithServer, async_send_request_callback_with_request) { - using SharedFutureWithRequest = - rclcpp::Client::SharedFutureWithRequest; - - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - - auto request = std::make_shared(); - bool received_response = false; - auto callback = [&request, &received_response](SharedFutureWithRequest future) { - auto request_response_pair = future.get(); - EXPECT_EQ(request, request_response_pair.first); - EXPECT_NE(nullptr, request_response_pair.second); - received_response = true; - }; - auto req_id = client->async_send_request(request, std::move(callback)); - - auto start = std::chrono::steady_clock::now(); - while (!received_response && - (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) - { - rclcpp::spin_some(node); - } - EXPECT_TRUE(received_response); - EXPECT_FALSE(client->remove_pending_request(req_id)); -} - -TEST_F(TestClientWithServer, test_client_remove_pending_request) { - auto client = node->create_client("no_service_server_available_here"); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - - EXPECT_TRUE(client->remove_pending_request(future)); -} - -TEST_F(TestClientWithServer, prune_requests_older_than_no_pruned) { - auto client = node->create_client(service_name); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - auto time = std::chrono::system_clock::now() + 1s; - - EXPECT_EQ(1u, client->prune_requests_older_than(time)); -} - -TEST_F(TestClientWithServer, prune_requests_older_than_with_pruned) { - auto client = node->create_client(service_name); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - auto time = std::chrono::system_clock::now() + 1s; - - std::vector pruned_requests; - EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests)); - ASSERT_EQ(1u, pruned_requests.size()); - EXPECT_EQ(future.request_id, pruned_requests[0]); -} - -TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) { - // Checking rcl_send_request in rclcpp::Client::async_send_request() - auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); - EXPECT_THROW(SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError); -} - -TEST_F(TestClientWithServer, async_send_request_rcl_service_server_is_available_error) { - { - // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready - auto client = node->create_client(service_name); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID); - EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); - } - { - // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready - auto client = node->create_client(service_name); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); - EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); - } - { - // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready - auto client = node->create_client(service_name); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); - EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); - } -} - -TEST_F(TestClientWithServer, take_response) { - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - auto request = std::make_shared(); - auto request_header = client->create_request_header(); - test_msgs::srv::Empty::Response response; - - client->async_send_request(request); - EXPECT_FALSE(client->take_response(response, *request_header.get())); - - { - // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_take_response, RCL_RET_OK); - EXPECT_TRUE(client->take_response(response, *request_header.get())); - } - { - // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED); - EXPECT_FALSE(client->take_response(response, *request_header.get())); - } - { - // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_take_response, RCL_RET_ERROR); - EXPECT_THROW( - client->take_response(response, *request_header.get()), - rclcpp::exceptions::RCLError); - } -} - -/* - Testing on_new_response callbacks. - */ -TEST_F(TestClient, on_new_response_callback) { - auto client_node = std::make_shared("client_node", "ns"); - auto server_node = std::make_shared("server_node", "ns"); - - rclcpp::ServicesQoS client_qos; - client_qos.keep_last(3); - auto client = client_node->create_client("test_service", client_qos); - std::atomic server_requests_count {0}; - auto server_callback = [&server_requests_count]( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {server_requests_count++;}; - auto server = server_node->create_service( - "test_service", server_callback, client_qos); - auto request = std::make_shared(); - - std::atomic c1 {0}; - auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; - client->set_on_new_response_callback(increase_c1_cb); - - client->async_send_request(request); - auto start = std::chrono::steady_clock::now(); - while (server_requests_count == 0 && - (std::chrono::steady_clock::now() - start) < 10s) - { - rclcpp::spin_some(server_node); - } - - ASSERT_EQ(server_requests_count, 1u); - - start = std::chrono::steady_clock::now(); - do { - std::this_thread::sleep_for(100ms); - } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); - - EXPECT_EQ(c1.load(), 1u); - - std::atomic c2 {0}; - auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; - client->set_on_new_response_callback(increase_c2_cb); - - client->async_send_request(request); - start = std::chrono::steady_clock::now(); - while (server_requests_count == 1 && - (std::chrono::steady_clock::now() - start) < 10s) - { - rclcpp::spin_some(server_node); - } - - ASSERT_EQ(server_requests_count, 2u); - - start = std::chrono::steady_clock::now(); - do { - std::this_thread::sleep_for(100ms); - } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); - - EXPECT_EQ(c1.load(), 1u); - EXPECT_EQ(c2.load(), 1u); - - client->clear_on_new_response_callback(); - - client->async_send_request(request); - client->async_send_request(request); - client->async_send_request(request); - start = std::chrono::steady_clock::now(); - while (server_requests_count < 5 && - (std::chrono::steady_clock::now() - start) < 10s) - { - rclcpp::spin_some(server_node); - } - - ASSERT_EQ(server_requests_count, 5u); - - std::atomic c3 {0}; - auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; - client->set_on_new_response_callback(increase_c3_cb); - - start = std::chrono::steady_clock::now(); - do { - std::this_thread::sleep_for(100ms); - } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); - - EXPECT_EQ(c1.load(), 1u); - EXPECT_EQ(c2.load(), 1u); - EXPECT_EQ(c3.load(), 3u); - - std::function invalid_cb = nullptr; - EXPECT_THROW(client->set_on_new_response_callback(invalid_cb), std::invalid_argument); -} - -TEST_F(TestClient, rcl_client_request_publisher_get_actual_qos_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); - auto client = node->create_client("service"); - RCLCPP_EXPECT_THROW_EQ( - client->get_request_publisher_actual_qos(), - std::runtime_error("failed to get client's request publisher qos settings: error not set")); -} - -TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); - auto client = node->create_client("service"); - RCLCPP_EXPECT_THROW_EQ( - client->get_response_subscription_actual_qos(), - std::runtime_error("failed to get client's response subscription qos settings: error not set")); -} - -TEST_F(TestClient, client_qos) { - rclcpp::ServicesQoS qos_profile; - qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); - rclcpp::Duration duration(std::chrono::nanoseconds(1)); - qos_profile.deadline(duration); - qos_profile.lifespan(duration); - qos_profile.liveliness_lease_duration(duration); - - auto client = - node->create_client("client", qos_profile); - - auto rp_qos = client->get_request_publisher_actual_qos(); - auto rs_qos = client->get_response_subscription_actual_qos(); - - EXPECT_EQ(qos_profile, rp_qos); - // Lifespan has no meaning for subscription/readers - rs_qos.lifespan(qos_profile.lifespan()); - EXPECT_EQ(qos_profile, rs_qos); -} - -TEST_F(TestClient, client_qos_depth) { - using namespace std::literals::chrono_literals; - - rclcpp::ServicesQoS client_qos_profile; - client_qos_profile.keep_last(2); - - auto client = node->create_client("test_qos_depth", client_qos_profile); - - uint64_t server_cb_count_ = 0; - auto server_callback = [&]( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; - - auto server_node = std::make_shared("server_node", "/ns"); - - rclcpp::QoS server_qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - - auto server = server_node->create_service( - "test_qos_depth", std::move(server_callback), server_qos); - - auto request = std::make_shared(); - ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); - - using SharedFuture = rclcpp::Client::SharedFuture; - uint64_t client_cb_count_ = 0; - auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { - if (nullptr == future_response.get()) { - request_result = ::testing::AssertionFailure() << "Future response was null"; - } - client_cb_count_++; - }; - - uint64_t client_requests = 5; - for (uint64_t i = 0; i < client_requests; i++) { - client->async_send_request(request, client_callback); - std::this_thread::sleep_for(10ms); - } - - auto start = std::chrono::steady_clock::now(); - while ((server_cb_count_ < client_requests) && - (std::chrono::steady_clock::now() - start) < 2s) - { - rclcpp::spin_some(server_node); - std::this_thread::sleep_for(2ms); - } - - EXPECT_GT(server_cb_count_, client_qos_profile.depth()); - - start = std::chrono::steady_clock::now(); - while ((client_cb_count_ < client_qos_profile.depth()) && - (std::chrono::steady_clock::now() - start) < 1s) - { - rclcpp::spin_some(node); - } - - // Spin an extra time to check if client QoS depth has been ignored, - // so more client callbacks might be called than expected. - rclcpp::spin_some(node); - - EXPECT_EQ(client_cb_count_, client_qos_profile.depth()); -} diff --git a/rclcpp/test/rclcpp/test_client_common.cpp b/rclcpp/test/rclcpp/test_client_common.cpp new file mode 100644 index 0000000000..65475bd8fc --- /dev/null +++ b/rclcpp/test/rclcpp/test_client_common.cpp @@ -0,0 +1,591 @@ +// Copyright 2024 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 + +#include +#include +#include + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rclcpp/create_generic_client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/srv/empty.hpp" + +template +class TestAllClientTypesWithServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "ns"); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + service = node->create_service(service_name, std::move(callback)); + } + + template + auto SendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + if constexpr (std::is_same_v) { + return GenericClientSendEmptyRequestAndWait(timeout); + } else if constexpr (std::is_same_v>) { + return ClientSendEmptyRequestAndWait(timeout); + } else { + return ::testing::AssertionFailure() << "No test for this client type"; + } + } + + ::testing::AssertionResult GenericClientSendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + auto client = node->create_generic_client(service_name, "test_msgs/srv/Empty"); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Service is not available yet"; + } + + auto request = std::make_shared(); + + auto future_and_req_id = client->async_send_request(request.get()); + + auto ret = rclcpp::spin_until_future_complete(node, future_and_req_id, timeout); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + + if (client->remove_pending_request(future_and_req_id.request_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } + + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult ClientSendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + using SharedFuture = rclcpp::Client::SharedFuture; + + auto client = node->create_client(service_name); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Waiting for service failed"; + } + + auto request = std::make_shared(); + bool received_response = false; + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto callback = [&received_response, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + received_response = true; + }; + + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < timeout) + { + rclcpp::spin_some(node); + } + + if (!received_response) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + if (client->remove_pending_request(req_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } + + return request_result; + } + + template + auto create_client( + rclcpp::Node::SharedPtr node, + const std::string service_name = "empty_service", + const rclcpp::QoS & qos = rclcpp::ServicesQoS()) + { + if constexpr (std::is_same_v) { + return node->create_generic_client(service_name, "test_msgs/srv/Empty", qos); + } else if constexpr (std::is_same_v>) { + return node->template create_client(service_name, qos); + } else { + ASSERT_TRUE(false) << "Not know how to create this kind of client"; + } + } + + template + auto async_send_request(std::shared_ptr client, std::shared_ptr request) + { + if constexpr (std::is_same_v) { + return client->async_send_request(request.get()); + } else if constexpr (std::is_same_v>) { + return client->async_send_request(request); + } else { + ASSERT_TRUE(false) << "Not know how to send request for this kind of client"; + } + } + + template + auto take_response( + std::shared_ptr client, + ResponseType & response, + std::shared_ptr request_header) + { + if constexpr (std::is_same_v) { + return client->take_response(static_cast(&response), *request_header.get()); + } else if constexpr (std::is_same_v>) { + return client->take_response(response, *request_header.get()); + } else { + ASSERT_TRUE(false) << "Not know how to take response for this kind of client"; + } + } + + std::shared_ptr node; + std::shared_ptr> service; + const std::string service_name{"empty_service"}; +}; + +using ClientType = + ::testing::Types< + rclcpp::Client, + rclcpp::GenericClient>; + +class ClientTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same_v>) { + return "Client"; + } + + if (std::is_same_v) { + return "GenericClient"; + } + + return ""; + } +}; + +TYPED_TEST_SUITE(TestAllClientTypesWithServer, ClientType, ClientTypeNames); + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request) +{ + using ClientType = TypeParam; + EXPECT_TRUE(this->template SendEmptyRequestAndWait()); +} + +TYPED_TEST(TestAllClientTypesWithServer, test_client_remove_pending_request) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future_and_req_id = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + + EXPECT_TRUE(client->remove_pending_request(future_and_req_id.request_id)); +} + +TYPED_TEST(TestAllClientTypesWithServer, prune_requests_older_than_no_pruned) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + auto time = std::chrono::system_clock::now() + std::chrono::seconds(1); + + EXPECT_EQ(1u, client->prune_requests_older_than(time)); +} + +TYPED_TEST(TestAllClientTypesWithServer, prune_requests_older_than_with_pruned) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + auto time = std::chrono::system_clock::now() + std::chrono::seconds(1); + + std::vector pruned_requests; + EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests)); + ASSERT_EQ(1u, pruned_requests.size()); + EXPECT_EQ(future.request_id, pruned_requests[0]); +} + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_rcl_send_request_error) +{ + using ClientType = TypeParam; + + // Checking rcl_send_request in rclcpp::Client::async_send_request() or + // rclcpp::GenericClient::async_send_request() + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); + EXPECT_THROW(this->template SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError); +} + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_rcl_service_server_is_available_error) +{ + using ClientType = TypeParam; + + { + // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } +} + +TYPED_TEST(TestAllClientTypesWithServer, take_response) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto request = std::make_shared(); + auto request_header = client->create_request_header(); + test_msgs::srv::Empty::Response response; + + this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + + EXPECT_FALSE(this->take_response(client, response, request_header)); + + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_OK); + EXPECT_TRUE(this->take_response(client, response, request_header)); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED); + EXPECT_FALSE(this->take_response(client, response, request_header)); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_ERROR); + EXPECT_THROW( + this->take_response(client, response, request_header), + rclcpp::exceptions::RCLError); + } +} + +/* + Testing on_new_response callbacks. + */ +TYPED_TEST(TestAllClientTypesWithServer, on_new_response_callback) +{ + using ClientType = TypeParam; + + auto client_node = std::make_shared("test_client_node", "ns"); + auto server_node = std::make_shared("test_server_node", "ns"); + + rclcpp::ServicesQoS client_qos; + client_qos.keep_last(3); + + auto client = this->template create_client(client_node, "test_service", client_qos); + + std::atomic server_requests_count {0}; + auto server_callback = [&server_requests_count]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_requests_count++;}; + auto server = server_node->create_service( + "test_service", server_callback, client_qos); + auto request = std::make_shared(); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + client->set_on_new_response_callback(increase_c1_cb); + + this->template async_send_request(client, request); + auto start = std::chrono::steady_clock::now(); + while (server_requests_count == 0 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 1u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + client->set_on_new_response_callback(increase_c2_cb); + + this->template async_send_request(client, request); + start = std::chrono::steady_clock::now(); + while (server_requests_count == 1 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 2u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + client->clear_on_new_response_callback(); + + this->template async_send_request(client, request); + this->template async_send_request(client, request); + this->template async_send_request(client, request); + start = std::chrono::steady_clock::now(); + while (server_requests_count < 5 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 5u); + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + client->set_on_new_response_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(client->set_on_new_response_callback(invalid_cb), std::invalid_argument); +} + +TYPED_TEST(TestAllClientTypesWithServer, client_qos) +{ + using ClientType = TypeParam; + + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); + + auto client = this->template create_client( + this->node, this->service_name, qos_profile); + + auto rp_qos = client->get_request_publisher_actual_qos(); + auto rs_qos = client->get_response_subscription_actual_qos(); + + EXPECT_EQ(qos_profile, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); +} + +TYPED_TEST(TestAllClientTypesWithServer, rcl_client_request_publisher_get_actual_qos_error) +{ + using ClientType = TypeParam; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); + auto client = this->template create_client(this->node, "service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_request_publisher_actual_qos(), + std::runtime_error("failed to get client's request publisher qos settings: error not set")); +} + +TYPED_TEST(TestAllClientTypesWithServer, rcl_client_response_subscription_get_actual_qos_error) +{ + using ClientType = TypeParam; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); + auto client = this->template create_client(this->node, "service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_response_subscription_actual_qos(), + std::runtime_error("failed to get client's response subscription qos settings: error not set")); +} + +// The following tests are only for rclcpp::Client +void client_async_send_request_callback_with_request( + rclcpp::Node::SharedPtr node, const std::string service_name) +{ + using SharedFutureWithRequest = + rclcpp::Client::SharedFutureWithRequest; + + auto client = node->create_client(service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + + auto request = std::make_shared(); + bool received_response = false; + auto callback = [&request, &received_response](SharedFutureWithRequest future) { + auto request_response_pair = future.get(); + EXPECT_EQ(request, request_response_pair.first); + EXPECT_NE(nullptr, request_response_pair.second); + received_response = true; + }; + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) + { + rclcpp::spin_some(node); + } + EXPECT_TRUE(received_response); + EXPECT_FALSE(client->remove_pending_request(req_id)); +} +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_callback_with_request) +{ + using ClientType = TypeParam; + + if (std::is_same_v>) { + client_async_send_request_callback_with_request(this->node, this->service_name); + } else if (std::is_same_v) { + GTEST_SKIP() << "Skipping test for GenericClient"; + } else { + GTEST_SKIP() << "Skipping test"; + } +} + +void client_qos_depth(rclcpp::Node::SharedPtr node) +{ + using namespace std::literals::chrono_literals; + + rclcpp::ServicesQoS client_qos_profile; + client_qos_profile.keep_last(2); + + auto client = node->create_client("test_qos_depth", client_qos_profile); + + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rclcpp::QoS server_qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos); + + auto request = std::make_shared(); + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + + using SharedFuture = rclcpp::Client::SharedFuture; + uint64_t client_cb_count_ = 0; + auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + client_cb_count_++; + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < client_requests) && + (std::chrono::steady_clock::now() - start) < 2s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(2ms); + } + + EXPECT_GT(server_cb_count_, client_qos_profile.depth()); + + start = std::chrono::steady_clock::now(); + while ((client_cb_count_ < client_qos_profile.depth()) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(node); + } + + // Spin an extra time to check if client QoS depth has been ignored, + // so more client callbacks might be called than expected. + rclcpp::spin_some(node); + + EXPECT_EQ(client_cb_count_, client_qos_profile.depth()); +} + +TYPED_TEST(TestAllClientTypesWithServer, qos_depth) +{ + using ClientType = TypeParam; + + if (std::is_same_v>) { + client_qos_depth(this->node); + } else if (std::is_same_v) { + GTEST_SKIP() << "Skipping test for GenericClient"; + } else { + GTEST_SKIP() << "Skipping test"; + } +} diff --git a/rclcpp/test/rclcpp/test_context.cpp b/rclcpp/test/rclcpp/test_context.cpp index 9d736f9db2..c8779371fe 100644 --- a/rclcpp/test/rclcpp/test_context.cpp +++ b/rclcpp/test/rclcpp/test_context.cpp @@ -214,3 +214,25 @@ TEST(TestContext, check_on_shutdown_callback_order_after_del) { EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0); } + +// This test checks that contexts will be properly destroyed when leaving a scope, after a +// guard condition has been created. +TEST(TestContext, check_context_destroyed) { + rclcpp::Context::SharedPtr ctx; + { + ctx = std::make_shared(); + ctx->init(0, nullptr); + + auto group = std::make_shared( + rclcpp::CallbackGroupType::MutuallyExclusive, + ctx->weak_from_this(), + false); + + rclcpp::GuardCondition::SharedPtr gc = group->get_notify_guard_condition(); + ASSERT_NE(gc, nullptr); + + ASSERT_EQ(ctx.use_count(), 1u); + } + + ASSERT_EQ(ctx.use_count(), 1u); +} diff --git a/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp b/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp new file mode 100644 index 0000000000..e3020efd1e --- /dev/null +++ b/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp @@ -0,0 +1,88 @@ +// 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. + +#include +#include "rclcpp/copy_all_parameter_values.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNode, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + bool override = false; + rclcpp::copy_all_parameter_values(node1, node2, override); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + // Test if parameter overrides are permissible that Node2's value is overridden + override = true; + rclcpp::copy_all_parameter_values(node1, node2, override); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("bar")); +} + +TEST_F(TestNode, TestParamCopyingExceptions) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for Parameter value conflicts handled + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(0.123)); + + bool override = true; + EXPECT_NO_THROW( + rclcpp::copy_all_parameter_values(node1, node2, override)); + + // Tests for Parameter read-only handled + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar")))); + node2->declare_parameter("Foo1", rclcpp::ParameterValue(0.123)); + EXPECT_NO_THROW(rclcpp::copy_all_parameter_values(node1, node2, override)); +} diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f253af4838..c4dc1f7e61 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -193,3 +193,29 @@ TEST(TestCreateTimer, timer_function_pointer) rclcpp::shutdown(); } + +TEST(TestCreateTimer, timer_without_autostart) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_create_timer_node"); + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + []() {}, + nullptr, + false); + + EXPECT_TRUE(timer->is_canceled()); + EXPECT_EQ(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + + timer->reset(); + EXPECT_LE(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer->is_canceled()); + + timer->cancel(); + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index d5908c7d4b..2347514d7a 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -79,6 +79,35 @@ TEST_F(TestDuration, operators) { EXPECT_TRUE(time == assignment_op_duration); } +TEST_F(TestDuration, operators_with_message_stamp) { + builtin_interfaces::msg::Time time_msg = rclcpp::Time(0, 100000000u); // 0.1s + rclcpp::Duration pos_duration(1, 100000000u); // 1.1s + rclcpp::Duration neg_duration(-2, 900000000u); // -1.1s + + builtin_interfaces::msg::Time res_addpos = time_msg + pos_duration; + EXPECT_EQ(res_addpos.sec, 1); + EXPECT_EQ(res_addpos.nanosec, 200000000u); + + builtin_interfaces::msg::Time res_addneg = time_msg + neg_duration; + EXPECT_EQ(res_addneg.sec, -1); + EXPECT_EQ(res_addneg.nanosec, 0); + + builtin_interfaces::msg::Time res_subpos = time_msg - pos_duration; + EXPECT_EQ(res_subpos.sec, -1); + EXPECT_EQ(res_subpos.nanosec, 0); + + builtin_interfaces::msg::Time res_subneg = time_msg - neg_duration; + EXPECT_EQ(res_subneg.sec, 1); + EXPECT_EQ(res_subneg.nanosec, 200000000u); + + builtin_interfaces::msg::Time neg_time_msg; + neg_time_msg.sec = -1; + auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + + EXPECT_THROW(neg_time_msg + max, std::runtime_error); + EXPECT_THROW(time_msg + max, std::overflow_error); +} + TEST_F(TestDuration, chrono_overloads) { int64_t ns = 123456789l; auto chrono_ns = std::chrono::nanoseconds(ns); diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index bdbb0a1079..668ab96797 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -46,23 +46,6 @@ class DummyExecutor : public rclcpp::Executor { spin_node_once_nanoseconds(node, std::chrono::milliseconds(100)); } - - rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr() - { - return memory_strategy_.get(); - } - - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group) - { - std::lock_guard guard_{mutex_}; // only to make the TSA happy - return get_node_by_group(weak_groups_to_nodes_, group); - } - - rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) - { - return get_group_by_timer(timer); - } }; class TestExecutor : public ::testing::Test @@ -130,7 +113,7 @@ TEST_F(TestExecutor, constructor_bad_wait_set_init) { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( static_cast(std::make_unique()), - std::runtime_error("Failed to create wait set in Executor constructor: error not set")); + std::runtime_error("Failed to create wait set: error not set")); } TEST_F(TestExecutor, add_callback_group_twice) { @@ -142,7 +125,7 @@ TEST_F(TestExecutor, add_callback_group_twice) { cb_group->get_associated_with_executor_atomic().exchange(false); RCLCPP_EXPECT_THROW_EQ( dummy.add_callback_group(cb_group, node->get_node_base_interface(), false), - std::runtime_error("Callback group was already added to executor.")); + std::runtime_error("Callback group has already been added to this executor.")); } TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { @@ -168,9 +151,15 @@ TEST_F(TestExecutor, remove_callback_group_null_node) { node.reset(); + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. RCLCPP_EXPECT_THROW_EQ( dummy.remove_callback_group(cb_group, false), std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(dummy.remove_callback_group(cb_group, false)); } TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { @@ -197,7 +186,7 @@ TEST_F(TestExecutor, remove_node_not_associated) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_node(node->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with an executor.")); + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); } TEST_F(TestExecutor, remove_node_associated_with_different_executor) { @@ -211,7 +200,7 @@ TEST_F(TestExecutor, remove_node_associated_with_different_executor) { RCLCPP_EXPECT_THROW_EQ( dummy2.remove_node(node1->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with this executor.")); + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); } TEST_F(TestExecutor, spin_node_once_nanoseconds) { @@ -328,42 +317,14 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { std::runtime_error("Failed to trigger guard condition in cancel: error not set")); } -TEST_F(TestExecutor, set_memory_strategy_nullptr) { - DummyExecutor dummy; - - RCLCPP_EXPECT_THROW_EQ( - dummy.set_memory_strategy(nullptr), - std::runtime_error("Received NULL memory strategy in executor.")); -} - -TEST_F(TestExecutor, set_memory_strategy) { - DummyExecutor dummy; - rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy = - std::make_shared< - rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); - - dummy.set_memory_strategy(strategy); - EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get()); -} - -TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); - - dummy.add_node(node); - // Wait for the wall timer to have expired. - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); +TEST_F(TestExecutor, create_executor_fail_wait_set_clear) { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( - dummy.spin_once(std::chrono::milliseconds(1)), - std::runtime_error( - "Failed to trigger guard condition from execute_any_executable: error not set")); + DummyExecutor dummy, + std::runtime_error("Couldn't clear the wait set: error not set")); } -TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { +TEST_F(TestExecutor, spin_all_fail_wait_set_clear) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); auto timer = @@ -371,9 +332,10 @@ TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { dummy.add_node(node); auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( - dummy.spin_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't clear wait set: error not set")); + dummy.spin_all(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't clear the wait set: error not set")); } TEST_F(TestExecutor, spin_some_fail_wait_set_resize) { @@ -401,7 +363,7 @@ TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) { RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.spin_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't fill wait set")); + std::runtime_error("Couldn't fill wait set: error not set")); } TEST_F(TestExecutor, spin_some_fail_wait) { @@ -417,71 +379,6 @@ TEST_F(TestExecutor, spin_some_fail_wait) { std::runtime_error("rcl_wait() failed: error not set")); } -TEST_F(TestExecutor, get_node_by_group_null_group) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr)); -} - -TEST_F(TestExecutor, get_node_by_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); - ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_node_by_group_not_found) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_nullptr) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr)); -} - -TEST_F(TestExecutor, get_group_by_timer) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - cb_group.reset(); - - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_add_callback_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); @@ -546,6 +443,40 @@ TEST_F(TestExecutor, spin_node_once_node) { EXPECT_TRUE(spin_called); } +TEST_F(TestExecutor, spin_node_all_base_interface) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_node_all_node) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node, std::chrono::milliseconds(50)); + EXPECT_TRUE(spin_called); +} + TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); diff --git a/rclcpp/test/rclcpp/test_generic_client.cpp b/rclcpp/test/rclcpp/test_generic_client.cpp new file mode 100644 index 0000000000..be65ea1f53 --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_client.cpp @@ -0,0 +1,230 @@ +// 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 + +#include +#include +#include + +#include "rclcpp/create_generic_client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +#include "../mocking_utils/patch.hpp" + +#include "test_msgs/srv/empty.hpp" + +using namespace std::chrono_literals; + +// All tests are from test_client + +class TestGenericClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestGenericClientSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestGenericClient, construction_and_destruction) { + { + auto client = node->create_generic_client("test_service", "test_msgs/srv/Empty"); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("invalid_test_service?", "test_msgs/srv/Empty"); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("test_service", "test_msgs/srv/InvalidType"); + }, std::runtime_error); + } +} + +TEST_F(TestGenericClient, construction_with_free_function) { + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "test_service", + "test_msgs/srv/InvalidType", + rclcpp::ServicesQoS(), + nullptr); + }, std::runtime_error); + } + { + auto client = rclcpp::create_generic_client( + node, + "test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node, + "invalid_?test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node, + "invalid_?test_service", + "test_msgs/srv/InvalidType", + rclcpp::ServicesQoS(), + nullptr); + }, std::runtime_error); + } +} + +TEST_F(TestGenericClient, construct_with_rcl_error) { + { + // reset() is not necessary for this exception, but handles unused return value warning + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_generic_client("test_service", "test_msgs/srv/Empty").reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR); + EXPECT_NO_THROW( + node->create_generic_client("test_service", "test_msgs/srv/Empty").reset()); + } +} + +TEST_F(TestGenericClient, wait_for_service) { + const std::string service_name = "test_service"; + + auto client = node->create_generic_client(service_name, "test_msgs/srv/Empty"); + EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0))); + EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10))); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto service = + node->create_service(service_name, std::move(callback)); + + EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1))); + EXPECT_TRUE(client->service_is_ready()); +} + +/* + Testing generic client construction and destruction for subnodes. + */ +TEST_F(TestGenericClientSub, construction_and_destruction) { + { + auto client = subnode->create_generic_client("test_service", "test_msgs/srv/Empty"); + EXPECT_STREQ(client->get_service_name(), "/ns/test_service"); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("invalid_service?", "test_msgs/srv/Empty"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index f4cef0b757..79fbfcc33a 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -69,7 +69,7 @@ class RclcppGenericNodeFixture : public Test size_t counter = 0; auto subscription = node_->create_generic_subscription( topic_name, type, rclcpp::QoS(1), - [&counter, &messages, this](std::shared_ptr message) { + [&counter, &messages, this](const std::shared_ptr message) { T2 deserialized_message; rclcpp::Serialization serializer; serializer.deserialize_message(message.get(), &deserialized_message); @@ -236,7 +236,7 @@ TEST_F(RclcppGenericNodeFixture, generic_subscription_uses_qos) auto publisher = node_->create_publisher(topic_name, qos); auto subscription = node_->create_generic_subscription( topic_name, topic_type, qos, - [](std::shared_ptr/* message */) {}); + [](std::shared_ptr/* message */) {}); auto connected = [publisher, subscription]() -> bool { return publisher->get_subscription_count() && subscription->get_publisher_count(); }; @@ -263,3 +263,49 @@ TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) // It normally takes < 20ms, 5s chosen as "a very long time" ASSERT_TRUE(wait_for(connected, 5s)); } + +TEST_F(RclcppGenericNodeFixture, generic_subscription_different_callbacks) +{ + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::QoS(1); + + auto publisher = node_->create_publisher(topic_name, qos); + + // Test shared_ptr for const messages + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](const std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } + + // Test unique_ptr + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](std::unique_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } + + // Test message callback + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](rclcpp::SerializedMessage /* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } +} diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 1e72264869..ed54074a4e 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -66,16 +66,6 @@ TEST_F(TestGuardCondition, construction_and_destruction) { } } -/* - * Testing context accessor. - */ -TEST_F(TestGuardCondition, get_context) { - { - auto gc = std::make_shared(); - gc->get_context(); - } -} - /* * Testing rcl guard condition accessor. */ @@ -115,19 +105,11 @@ TEST_F(TestGuardCondition, add_to_wait_set) { "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_OK); rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - EXPECT_NO_THROW(gc->add_to_wait_set(&wait_set)); - EXPECT_NO_THROW(gc->add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(gc->add_to_wait_set(wait_set)); + EXPECT_NO_THROW(gc->add_to_wait_set(wait_set)); rcl_wait_set_t wait_set_2 = rcl_get_zero_initialized_wait_set(); - EXPECT_THROW(gc->add_to_wait_set(&wait_set_2), std::runtime_error); - } - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); - - auto gd = std::make_shared(); - EXPECT_THROW(gd->add_to_wait_set(nullptr), rclcpp::exceptions::RCLError); + EXPECT_THROW(gc->add_to_wait_set(wait_set_2), std::runtime_error); } } } diff --git a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp index 16c457c96f..fa427de74b 100644 --- a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp @@ -190,6 +190,33 @@ TEST(TestIntraProcessBuffer, shared_buffer_consume) { EXPECT_EQ(1L, original_shared_msg.use_count()); EXPECT_EQ(*original_shared_msg, *popped_unique_msg); EXPECT_NE(original_message_pointer, popped_message_pointer); + + original_shared_msg = std::make_shared('c'); + original_message_pointer = reinterpret_cast(original_shared_msg.get()); + auto original_shared_msg_2 = std::make_shared('d'); + auto original_message_pointer_2 = reinterpret_cast(original_shared_msg_2.get()); + intra_process_buffer.add_shared(original_shared_msg); + intra_process_buffer.add_shared(original_shared_msg_2); + + auto shared_data_vec = intra_process_buffer.get_all_data_shared(); + EXPECT_EQ(2L, shared_data_vec.size()); + EXPECT_EQ(3L, original_shared_msg.use_count()); + EXPECT_EQ(original_shared_msg.use_count(), shared_data_vec[0].use_count()); + EXPECT_EQ(*original_shared_msg, *shared_data_vec[0]); + EXPECT_EQ(original_message_pointer, reinterpret_cast(shared_data_vec[0].get())); + EXPECT_EQ(3L, original_shared_msg_2.use_count()); + EXPECT_EQ(original_shared_msg_2.use_count(), shared_data_vec[1].use_count()); + EXPECT_EQ(*original_shared_msg_2, *shared_data_vec[1]); + EXPECT_EQ(original_message_pointer_2, reinterpret_cast(shared_data_vec[1].get())); + + auto unique_data_vec = intra_process_buffer.get_all_data_unique(); + EXPECT_EQ(2L, unique_data_vec.size()); + EXPECT_EQ(3L, original_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *unique_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(unique_data_vec[0].get())); + EXPECT_EQ(3L, original_shared_msg_2.use_count()); + EXPECT_EQ(*original_shared_msg_2, *unique_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(unique_data_vec[1].get())); } /* @@ -237,4 +264,103 @@ TEST(TestIntraProcessBuffer, unique_buffer_consume) { EXPECT_EQ(original_value, *popped_unique_msg); EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('c'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + auto original_unique_msg_2 = std::make_unique('d'); + auto original_message_pointer_2 = reinterpret_cast(original_unique_msg.get()); + auto original_value_2 = *original_unique_msg_2; + intra_process_buffer.add_unique(std::move(original_unique_msg)); + intra_process_buffer.add_unique(std::move(original_unique_msg_2)); + + auto shared_data_vec = intra_process_buffer.get_all_data_shared(); + EXPECT_EQ(2L, shared_data_vec.size()); + EXPECT_EQ(1L, shared_data_vec[0].use_count()); + EXPECT_EQ(original_value, *shared_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(shared_data_vec[0].get())); + EXPECT_EQ(1L, shared_data_vec[1].use_count()); + EXPECT_EQ(original_value_2, *shared_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(shared_data_vec[1].get())); + + auto unique_data_vec = intra_process_buffer.get_all_data_unique(); + EXPECT_EQ(2L, unique_data_vec.size()); + EXPECT_EQ(1L, shared_data_vec[0].use_count()); + EXPECT_EQ(original_value, *unique_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(unique_data_vec[0].get())); + EXPECT_EQ(1L, shared_data_vec[1].use_count()); + EXPECT_EQ(original_value_2, *unique_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(unique_data_vec[1].get())); +} + +/* + Check the available buffer capacity while storing and consuming data from an intra-process + buffer. + The initial available buffer capacity should equal the buffer size. + Inserting a message should decrease the available buffer capacity by 1. + Consuming a message should increase the available buffer capacity by 1. + */ +TEST(TestIntraProcessBuffer, available_capacity) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + constexpr auto history_depth = 5u; + + auto buffer_impl = + std::make_unique>( + history_depth); + + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + + auto original_unique_msg = std::make_unique('a'); + auto original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + EXPECT_EQ(history_depth - 1u, intra_process_buffer.available_capacity()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + auto second_unique_msg = std::make_unique('c'); + auto second_message_pointer = reinterpret_cast(second_unique_msg.get()); + auto second_value = *second_unique_msg; + + intra_process_buffer.add_unique(std::move(second_unique_msg)); + + EXPECT_EQ(history_depth - 2u, intra_process_buffer.available_capacity()); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(history_depth - 1u, intra_process_buffer.available_capacity()); + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + EXPECT_EQ(second_value, *popped_unique_msg); + EXPECT_EQ(second_message_pointer, popped_message_pointer); } diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 45d916b004..046a5228da 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -31,6 +31,111 @@ // NOLINTNEXTLINE(build/include_order) #include +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ +namespace mock +{ + +class IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + + virtual ~IntraProcessBufferBase() {} +}; + +} // namespace mock +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ +namespace mock +{ +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete> +class IntraProcessBuffer : public IntraProcessBufferBase +{ +public: + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) + + IntraProcessBuffer() + {} + + void add(ConstMessageSharedPtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + shared_msg = msg; + ++num_msgs; + } + + void add(MessageUniquePtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + unique_msg = std::move(msg); + ++num_msgs; + } + + void pop(std::uintptr_t & msg_ptr) + { + msg_ptr = message_ptr; + message_ptr = 0; + --num_msgs; + } + + size_t size() const + { + return num_msgs; + } + + std::vector get_all_data_shared() + { + if (shared_msg) { + return {shared_msg}; + } else if (unique_msg) { + return {std::make_shared(*unique_msg)}; + } + return {}; + } + + std::vector get_all_data_unique() + { + std::vector result; + if (shared_msg) { + result.push_back(std::make_unique(*shared_msg)); + } else if (unique_msg) { + result.push_back(std::make_unique(*unique_msg)); + } + return result; + } + + // need to store the messages somewhere otherwise the memory address will be reused + ConstMessageSharedPtr shared_msg; + MessageUniquePtr unique_msg; + + std::uintptr_t message_ptr; + // count add and pop + size_t num_msgs = 0u; +}; + +} // namespace mock +} // namespace buffers +} // namespace experimental +} // namespace rclcpp namespace rclcpp { // forward declaration @@ -80,6 +185,12 @@ class PublisherBase return qos_profile; } + bool + is_durability_transient_local() const + { + return qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal; + } + bool operator==(const rmw_gid_t & gid) const { @@ -117,6 +228,9 @@ class Publisher : public PublisherBase { auto allocator = std::make_shared(); message_allocator_ = std::make_shared(*allocator.get()); + if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + buffer = std::make_shared>(); + } } // The following functions use the IntraProcessManager @@ -124,65 +238,12 @@ class Publisher : public PublisherBase void publish(MessageUniquePtr msg); std::shared_ptr message_allocator_; + typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::SharedPtr buffer{nullptr}; }; } // namespace mock } // namespace rclcpp -namespace rclcpp -{ -namespace experimental -{ -namespace buffers -{ -namespace mock -{ -template< - typename MessageT, - typename Alloc = std::allocator, - typename MessageDeleter = std::default_delete> -class IntraProcessBuffer -{ -public: - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; - - RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) - - IntraProcessBuffer() - {} - - void add(ConstMessageSharedPtr msg) - { - message_ptr = reinterpret_cast(msg.get()); - shared_msg = msg; - } - - void add(MessageUniquePtr msg) - { - message_ptr = reinterpret_cast(msg.get()); - unique_msg = std::move(msg); - } - - void pop(std::uintptr_t & msg_ptr) - { - msg_ptr = message_ptr; - message_ptr = 0; - } - - // need to store the messages somewhere otherwise the memory address will be reused - ConstMessageSharedPtr shared_msg; - MessageUniquePtr unique_msg; - - std::uintptr_t message_ptr; -}; - -} // namespace mock -} // namespace buffers -} // namespace experimental -} // namespace rclcpp - - namespace rclcpp { namespace experimental @@ -221,6 +282,16 @@ class SubscriptionIntraProcessBase return topic_name.c_str(); } + bool + is_durability_transient_local() const + { + return qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal; + } + + virtual + size_t + available_capacity() const = 0; + rclcpp::QoS qos_profile; std::string topic_name; }; @@ -275,11 +346,17 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase } bool - use_take_shared_method() const + use_take_shared_method() const override { return take_shared_method; } + size_t + available_capacity() const override + { + return qos_profile.depth() - buffer->size(); + } + bool take_shared_method; typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::UniquePtr buffer; @@ -311,12 +388,14 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< // Prevent the header files of the mocked classes to be included #define RCLCPP__PUBLISHER_HPP_ #define RCLCPP__PUBLISHER_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ // Force ipm to use our mock publisher class. #define Publisher mock::Publisher #define PublisherBase mock::PublisherBase +#define IntraProcessBufferBase mock::IntraProcessBufferBase #define IntraProcessBuffer mock::IntraProcessBuffer #define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase #define SubscriptionIntraProcessBuffer mock::SubscriptionIntraProcessBuffer @@ -348,28 +427,36 @@ void Publisher::publish(MessageUniquePtr msg) throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( - intra_process_publisher_id_, - std::move(msg), - *message_allocator_); + if (buffer) { + auto shared_msg = ipm->template do_intra_process_publish_and_return_shared( + intra_process_publisher_id_, + std::move(msg), + *message_allocator_); + buffer->add(shared_msg); + } else { + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + *message_allocator_); + } } } // namespace mock } // namespace rclcpp /* - This tests how the class connects and disconnects publishers and subscriptions: - - Creates 2 publishers on different topics and a subscription to one of them. - Add everything to the intra-process manager. - - All the entities are expected to have different ids. - - Check the subscriptions count for each publisher. - - One of the publishers is expected to have 1 subscription, while the other 0. - - Check the subscription count for a non existing publisher id, should return 0. - - Add a new publisher and a new subscription both with reliable QoS. - - The subscriptions count of the previous publisher is expected to remain unchanged, - while the new publisher is expected to have 2 subscriptions (it's compatible with both QoS). - - Remove the just added subscriptions. - - The count for the last publisher is expected to decrease to 1. + * This tests how the class connects and disconnects publishers and subscriptions: + * - Creates 2 publishers on different topics and a subscription to one of them. + * Add everything to the intra-process manager. + * - All the entities are expected to have different ids. + * - Check the subscriptions count for each publisher. + * - One of the publishers is expected to have 1 subscription, while the other 0. + * - Check the subscription count for a non existing publisher id, should return 0. + * - Add a new publisher and a new subscription both with reliable QoS. + * - The subscriptions count of the previous publisher is expected to remain unchanged, + * while the new publisher is expected to have 2 subscriptions (it's compatible with both QoS). + * - Remove the just added subscriptions. + * - The count for the last publisher is expected to decrease to 1. */ TEST(TestIntraProcessManager, add_pub_sub) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -388,7 +475,7 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto p1_id = ipm->add_publisher(p1); auto p2_id = ipm->add_publisher(p2); - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); bool unique_ids = p1_id != p2_id && p2_id != s1_id; ASSERT_TRUE(unique_ids); @@ -404,7 +491,7 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto s2 = std::make_shared(rclcpp::QoS(10).reliable()); - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto p3_id = ipm->add_publisher(p3); p1_subs = ipm->get_subscription_count(p1_id); @@ -424,14 +511,14 @@ TEST(TestIntraProcessManager, add_pub_sub) { } /* - This tests the minimal usage of the class where there is a single subscription per publisher: - - Publishes a unique_ptr message with a subscription requesting ownership. - - The received message is expected to be the same. - - Remove the first subscription from ipm and add a new one. - - Publishes a unique_ptr message with a subscription not requesting ownership. - - The received message is expected to be the same, the first subscription do not receive it. - - Publishes a shared_ptr message with a subscription not requesting ownership. - - The received message is expected to be the same. + * This tests the minimal usage of the class where there is a single subscription per publisher: + * - Publishes a unique_ptr message with a subscription requesting ownership. + * - The received message is expected to be the same. + * - Remove the first subscription from ipm and add a new one. + * - Publishes a unique_ptr message with a subscription not requesting ownership. + * - The received message is expected to be the same, the first subscription do not receive it. + * - Publishes a shared_ptr message with a subscription not requesting ownership. + * - The received message is expected to be the same. */ TEST(TestIntraProcessManager, single_subscription) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -447,7 +534,7 @@ TEST(TestIntraProcessManager, single_subscription) { auto s1 = std::make_shared(); s1->take_shared_method = false; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -458,7 +545,7 @@ TEST(TestIntraProcessManager, single_subscription) { ipm->remove_subscription(s1_id); auto s2 = std::make_shared(); s2->take_shared_method = true; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); (void)s2_id; unique_msg = std::make_unique(); @@ -477,15 +564,15 @@ TEST(TestIntraProcessManager, single_subscription) { } /* - This tests the usage of the class where there are multiple subscriptions of the same type: - - Publishes a unique_ptr message with 2 subscriptions requesting ownership. - - One is expected to receive the published message, while the other will receive a copy. - - Publishes a unique_ptr message with 2 subscriptions not requesting ownership. - - Both received messages are expected to be the same as the published one. - - Publishes a shared_ptr message with 2 subscriptions requesting ownership. - - Both received messages are expected to be a copy of the published one. - - Publishes a shared_ptr message with 2 subscriptions not requesting ownership. - - Both received messages are expected to be the same as the published one. + * This tests the usage of the class where there are multiple subscriptions of the same type: + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership. + * - One is expected to receive the published message, while the other will receive a copy. + * - Publishes a unique_ptr message with 2 subscriptions not requesting ownership. + * - Both received messages are expected to be the same as the published one. + * - Publishes a shared_ptr message with 2 subscriptions requesting ownership. + * - Both received messages are expected to be a copy of the published one. + * - Publishes a shared_ptr message with 2 subscriptions not requesting ownership. + * - Both received messages are expected to be the same as the published one. */ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -501,11 +588,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s1 = std::make_shared(); s1->take_shared_method = false; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); auto s2 = std::make_shared(); s2->take_shared_method = false; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -521,11 +608,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s3 = std::make_shared(); s3->take_shared_method = true; - auto s3_id = ipm->add_subscription(s3); + auto s3_id = ipm->template add_subscription(s3); auto s4 = std::make_shared(); s4->take_shared_method = true; - auto s4_id = ipm->add_subscription(s4); + auto s4_id = ipm->template add_subscription(s4); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -540,11 +627,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s5 = std::make_shared(); s5->take_shared_method = false; - auto s5_id = ipm->add_subscription(s5); + auto s5_id = ipm->template add_subscription(s5); auto s6 = std::make_shared(); s6->take_shared_method = false; - auto s6_id = ipm->add_subscription(s6); + auto s6_id = ipm->template add_subscription(s6); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -560,12 +647,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s7 = std::make_shared(); s7->take_shared_method = true; - auto s7_id = ipm->add_subscription(s7); + auto s7_id = ipm->template add_subscription(s7); (void)s7_id; auto s8 = std::make_shared(); s8->take_shared_method = true; - auto s8_id = ipm->add_subscription(s8); + auto s8_id = ipm->template add_subscription(s8); (void)s8_id; unique_msg = std::make_unique(); @@ -578,20 +665,20 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { } /* - This tests the usage of the class where there are multiple subscriptions of different types: - - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not. - - The one requesting ownership is expected to receive the published message, - while the other is expected to receive a copy. - - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 1 not. - - One of the subscriptions requesting ownership is expected to receive the published message, - while both other subscriptions are expected to receive different copies. - - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 2 not. - - The 2 subscriptions not requesting ownership are expected to both receive the same copy - of the message, one of the subscription requesting ownership is expected to receive a - different copy, while the last is expected to receive the published message. - - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not. - - The subscription requesting ownership is expected to receive a copy of the message, while - the other is expected to receive the published message + * This tests the usage of the class where there are multiple subscriptions of different types: + * - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not. + * - The one requesting ownership is expected to receive the published message, + * while the other is expected to receive a copy. + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 1 not. + * - One of the subscriptions requesting ownership is expected to receive the published message, + * while both other subscriptions are expected to receive different copies. + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 2 not. + * - The 2 subscriptions not requesting ownership are expected to both receive the same copy + * of the message, one of the subscription requesting ownership is expected to receive a + * different copy, while the last is expected to receive the published message. + * - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not. + * - The subscription requesting ownership is expected to receive a copy of the message, while + * the other is expected to receive the published message */ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -607,11 +694,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s1 = std::make_shared(); s1->take_shared_method = true; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); auto s2 = std::make_shared(); s2->take_shared_method = false; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -626,15 +713,15 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s3 = std::make_shared(); s3->take_shared_method = false; - auto s3_id = ipm->add_subscription(s3); + auto s3_id = ipm->template add_subscription(s3); auto s4 = std::make_shared(); s4->take_shared_method = false; - auto s4_id = ipm->add_subscription(s4); + auto s4_id = ipm->template add_subscription(s4); auto s5 = std::make_shared(); s5->take_shared_method = true; - auto s5_id = ipm->add_subscription(s5); + auto s5_id = ipm->template add_subscription(s5); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -658,19 +745,19 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s6 = std::make_shared(); s6->take_shared_method = true; - auto s6_id = ipm->add_subscription(s6); + auto s6_id = ipm->template add_subscription(s6); auto s7 = std::make_shared(); s7->take_shared_method = true; - auto s7_id = ipm->add_subscription(s7); + auto s7_id = ipm->template add_subscription(s7); auto s8 = std::make_shared(); s8->take_shared_method = false; - auto s8_id = ipm->add_subscription(s8); + auto s8_id = ipm->template add_subscription(s8); auto s9 = std::make_shared(); s9->take_shared_method = false; - auto s9_id = ipm->add_subscription(s9); + auto s9_id = ipm->template add_subscription(s9); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -696,12 +783,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s10 = std::make_shared(); s10->take_shared_method = false; - auto s10_id = ipm->add_subscription(s10); + auto s10_id = ipm->template add_subscription(s10); (void)s10_id; auto s11 = std::make_shared(); s11->take_shared_method = true; - auto s11_id = ipm->add_subscription(s11); + auto s11_id = ipm->template add_subscription(s11); (void)s11_id; unique_msg = std::make_unique(); @@ -712,3 +799,170 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { EXPECT_EQ(original_message_pointer, received_message_pointer_10); EXPECT_NE(original_message_pointer, received_message_pointer_11); } + +/* + * This tests the method "lowest_available_capacity": + * - Creates 1 publisher. + * - The available buffer capacity should be at least history size. + * - Add 2 subscribers. + * - Add everything to the intra-process manager. + * - All the entities are expected to have different ids. + * - Check the subscriptions count for the publisher. + * - The available buffer capacity should be the history size. + * - Publish one message (without receiving it). + * - The available buffer capacity should decrease by 1. + * - Publish another message (without receiving it). + * - The available buffer capacity should decrease by 1. + * - One subscriber receives one message. + * - The available buffer capacity should stay the same, + * as the other subscriber still has not freed its buffer. + * - The other subscriber receives one message. + * - The available buffer capacity should increase by 1. + * - One subscription goes out of scope. + * - The available buffer capacity should not change. + */ +TEST(TestIntraProcessManager, lowest_available_capacity) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + + auto s1 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + auto s2 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto c1 = ipm->lowest_available_capacity(p1_id); + + ASSERT_LE(0u, c1); + + auto s1_id = ipm->template add_subscription(s1); + auto s2_id = ipm->template add_subscription(s2); + + bool unique_ids = s1_id != s2_id && p1_id != s1_id; + ASSERT_TRUE(unique_ids); + + size_t p1_subs = ipm->get_subscription_count(p1_id); + size_t non_existing_pub_subs = ipm->get_subscription_count(42); + ASSERT_EQ(2u, p1_subs); + ASSERT_EQ(0u, non_existing_pub_subs); + + c1 = ipm->lowest_available_capacity(p1_id); + auto non_existing_pub_c = ipm->lowest_available_capacity(42); + + ASSERT_EQ(history_depth, c1); + ASSERT_EQ(0u, non_existing_pub_c); + + auto unique_msg = std::make_unique(); + p1->publish(std::move(unique_msg)); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); + + unique_msg = std::make_unique(); + p1->publish(std::move(unique_msg)); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 2u, c1); + + s1->pop(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 2u, c1); + + s2->pop(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); + + ipm->get_subscription_intra_process(s1_id).reset(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); +} + +/* + * This tests the check inside add_publisher for transient_local + * durability publishers + * - add_publisher should throw runtime_error when no valid buffer ptr + * is passed with a transient_local publisher + */ +TEST(TestIntraProcessManager, transient_local_invalid_buffer) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(rclcpp::QoS(history_depth).transient_local()); + + ASSERT_THROW( + { + ipm->add_publisher(p1, nullptr); + }, std::runtime_error); +} + +/* + * This tests publishing function for transient_local durability publihers + * - A message is published before three transient_local subscriptions are added to + * ipm. Two of the subscriptions use take_shared method. Delivery of the message is verified + * along with the contents and pointer addresses from the subscriptions. + */ +TEST(TestIntraProcessManager, transient_local) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(rclcpp::QoS(history_depth).transient_local()); + + auto s1 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto s2 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto s3 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + + s1->take_shared_method = false; + s2->take_shared_method = true; + s3->take_shared_method = true; + + auto p1_id = ipm->add_publisher(p1, p1->buffer); + + p1->set_intra_process_manager(p1_id, ipm); + + auto unique_msg = std::make_unique(); + unique_msg->msg = "Test"; + p1->publish(std::move(unique_msg)); + + ipm->template add_subscription(s1); + ipm->template add_subscription(s2); + ipm->template add_subscription(s3); + + auto received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + auto received_message_pointer_3 = s3->pop(); + ASSERT_NE(0u, received_message_pointer_1); + ASSERT_NE(0u, received_message_pointer_2); + ASSERT_NE(0u, received_message_pointer_3); + ASSERT_EQ(received_message_pointer_3, received_message_pointer_2); + ASSERT_EQ( + reinterpret_cast(received_message_pointer_1)->msg, + reinterpret_cast(received_message_pointer_2)->msg); + ASSERT_EQ( + reinterpret_cast(received_message_pointer_1)->msg, + reinterpret_cast(received_message_pointer_3)->msg); + ASSERT_EQ("Test", reinterpret_cast(received_message_pointer_1)->msg); +} diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index e96729a2a1..348b7f0143 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -35,10 +35,10 @@ typedef std::map take_data() override {return nullptr;} - void execute(std::shared_ptr & data) override {(void)data;} + void execute(const std::shared_ptr &) override {} }; class TestMemoryStrategy : public ::testing::Test @@ -143,9 +143,9 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) { { auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; const rclcpp::QoS qos(10); weak_groups_to_nodes.insert( std::paircreate_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; const rclcpp::QoS qos(10); service = node->create_service( diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 512c2c1384..ad73aadc2a 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -78,6 +78,7 @@ TEST_F(TestNode, construction_and_destruction) { EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options()); EXPECT_NE(nullptr, node->get_graph_event()); EXPECT_NE(nullptr, node->get_clock()); + EXPECT_NE(nullptr, node->get_node_type_descriptions_interface()); } { @@ -3310,6 +3311,9 @@ TEST_F(TestNode, get_entity_names) { const auto service_names_and_types = node->get_service_names_and_types(); EXPECT_EQ(service_names_and_types.end(), service_names_and_types.find("service")); + EXPECT_EQ(0u, node->count_clients("service")); + EXPECT_EQ(0u, node->count_services("service")); + const auto service_names_and_types_by_node = node->get_service_names_and_types_by_node("node", "/ns"); EXPECT_EQ( diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 2ce414d327..afc48a652b 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -59,6 +59,8 @@ class TestParameterClient : public ::testing::Test node_with_option.reset(); } + // "start_type_description_service" and "use_sim_time" + const uint64_t builtin_param_count = 2; rclcpp::Node::SharedPtr node; rclcpp::Node::SharedPtr node_with_option; }; @@ -925,6 +927,7 @@ TEST_F(TestParameterClient, sync_parameter_delete_parameters) { Coverage for async load_parameters */ TEST_F(TestParameterClient, async_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -944,12 +947,13 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(5)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); } /* Coverage for sync load_parameters */ TEST_F(TestParameterClient, sync_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -964,13 +968,14 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { ASSERT_EQ(load_future[0].successful, true); // list parameters auto list_parameters = synchronous_client->list_parameters({}, 3); - ASSERT_EQ(list_parameters.names.size(), static_cast(5)); + ASSERT_EQ(list_parameters.names.size(), static_cast(expected_param_count)); } /* Coverage for async load_parameters with complicated regex expression */ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { + const uint64_t expected_param_count = 5 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -990,7 +995,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); // to check the parameter "a_value" std::string param_name = "a_value"; auto param = load_node->get_parameter(param_name); @@ -1020,6 +1025,7 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { Coverage for async load_parameters from maps with complicated regex expression */ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { + const uint64_t expected_param_count = 5 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -1068,7 +1074,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); // to check the parameter "a_value" std::string param_name = "a_value"; auto param = load_node->get_parameter(param_name); diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 20a46194fc..4284ea48bb 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -196,11 +196,7 @@ static std::vector invalid_qos_profiles() { std::vector parameters; - parameters.reserve(2); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); + parameters.reserve(1); parameters.push_back( TestParameters( rclcpp::QoS(rclcpp::KeepAll()), @@ -482,7 +478,7 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher && loaned_msg) { - this->do_loaned_message_publish(std::move(loaned_msg.release())); + this->do_loaned_message_publish(loaned_msg.release()); } void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const @@ -530,7 +526,7 @@ TEST_F(TestPublisher, run_event_handlers) { for (const auto & key_event_pair : publisher->get_event_handlers()) { auto handler = key_event_pair.second; - std::shared_ptr data = handler->take_data(); + const std::shared_ptr data = handler->take_data(); handler->execute(data); } } @@ -629,6 +625,41 @@ TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(6000))); } +TEST_F(TestPublisher, lowest_available_ipm_capacity) { + constexpr auto history_depth = 10u; + + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + + rclcpp::PublisherOptionsWithAllocator> options_ipm_disabled; + options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + rclcpp::PublisherOptionsWithAllocator> options_ipm_enabled; + options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto do_nothing = [](std::shared_ptr) {}; + auto pub_ipm_disabled = node->create_publisher( + "topic", history_depth, + options_ipm_disabled); + auto pub_ipm_enabled = node->create_publisher( + "topic", history_depth, + options_ipm_enabled); + auto sub = node->create_subscription( + "topic", + history_depth, + do_nothing); + + ASSERT_EQ(1, pub_ipm_enabled->get_intra_process_subscription_count()); + ASSERT_EQ(0, pub_ipm_disabled->lowest_available_ipm_capacity()); + ASSERT_EQ(history_depth, pub_ipm_enabled->lowest_available_ipm_capacity()); + + auto msg = std::make_shared(); + ASSERT_NO_THROW(pub_ipm_disabled->publish(*msg)); + ASSERT_NO_THROW(pub_ipm_enabled->publish(*msg)); + + ASSERT_EQ(0, pub_ipm_disabled->lowest_available_ipm_capacity()); + ASSERT_EQ(history_depth - 1u, pub_ipm_enabled->lowest_available_ipm_capacity()); +} + INSTANTIATE_TEST_SUITE_P( TestWaitForAllAckedWithParm, TestPublisherWaitForAllAcked, @@ -639,3 +670,96 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::QoS(1).best_effort(), rclcpp::QoS(1).best_effort()), std::pair( rclcpp::QoS(1).reliable(), rclcpp::QoS(1).best_effort()))); + +TEST_F(TestPublisher, intra_process_transient_local) { + constexpr auto history_depth = 10u; + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::PublisherOptionsWithAllocator> pub_options_ipm_disabled; + pub_options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + rclcpp::PublisherOptionsWithAllocator> pub_options_ipm_enabled; + pub_options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto pub_ipm_enabled_transient_local_enabled = node->create_publisher( + "topic1", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), pub_options_ipm_enabled); + auto pub_ipm_disabled_transient_local_enabled = node->create_publisher( + "topic2", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), pub_options_ipm_disabled); + auto pub_ipm_enabled_transient_local_disabled = node->create_publisher( + "topic3", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), pub_options_ipm_enabled); + auto pub_ipm_disabled_transient_local_disabled = node->create_publisher( + "topic4", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), pub_options_ipm_disabled); + + test_msgs::msg::Empty msg; + pub_ipm_enabled_transient_local_enabled->publish(msg); + pub_ipm_disabled_transient_local_enabled->publish(msg); + pub_ipm_enabled_transient_local_disabled->publish(msg); + pub_ipm_disabled_transient_local_disabled->publish(msg); + + auto do_nothing = [](std::shared_ptr) {}; + struct IntraProcessCallback + { + void callback_fun(size_t s) + { + (void) s; + called = true; + } + bool called = false; + }; + rclcpp::SubscriptionOptions sub_options_ipm_disabled; + sub_options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + rclcpp::SubscriptionOptions sub_options_ipm_enabled; + sub_options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + IntraProcessCallback callback1, callback2, callback3, callback4; + auto sub_ipm_enabled_transient_local_enabled = node->create_subscription( + "topic1", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), + do_nothing, sub_options_ipm_enabled); + sub_ipm_enabled_transient_local_enabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback1, std::placeholders::_1)); + auto sub_ipm_disabled_transient_local_enabled = node->create_subscription( + "topic2", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), + do_nothing, sub_options_ipm_disabled); + sub_ipm_disabled_transient_local_enabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback2, std::placeholders::_1)); + auto sub_ipm_enabled_transient_local_disabled = node->create_subscription( + "topic3", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), + do_nothing, sub_options_ipm_enabled); + sub_ipm_enabled_transient_local_disabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback3, std::placeholders::_1)); + auto sub_ipm_disabled_transient_local_disabled = node->create_subscription( + "topic4", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), + do_nothing, sub_options_ipm_disabled); + sub_ipm_disabled_transient_local_disabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback4, std::placeholders::_1)); + + EXPECT_TRUE(pub_ipm_enabled_transient_local_enabled->is_durability_transient_local()); + EXPECT_TRUE(pub_ipm_disabled_transient_local_enabled->is_durability_transient_local()); + EXPECT_FALSE(pub_ipm_enabled_transient_local_disabled->is_durability_transient_local()); + EXPECT_FALSE(pub_ipm_disabled_transient_local_disabled->is_durability_transient_local()); + + EXPECT_EQ(1, pub_ipm_enabled_transient_local_enabled->get_intra_process_subscription_count()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_enabled->get_intra_process_subscription_count()); + EXPECT_EQ(1, pub_ipm_enabled_transient_local_disabled->get_intra_process_subscription_count()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_disabled->get_intra_process_subscription_count()); + + EXPECT_EQ( + history_depth - 1u, + pub_ipm_enabled_transient_local_enabled->lowest_available_ipm_capacity()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_enabled->lowest_available_ipm_capacity()); + EXPECT_EQ( + history_depth, + pub_ipm_enabled_transient_local_disabled->lowest_available_ipm_capacity()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_disabled->lowest_available_ipm_capacity()); + + EXPECT_TRUE(callback1.called); + EXPECT_FALSE(callback2.called); + EXPECT_FALSE(callback3.called); + EXPECT_FALSE(callback4.called); +} diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index c041d86aee..9367a89014 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include +#include #include #include #include @@ -25,6 +25,7 @@ #include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/msg/large_message.hpp" #include "rclcpp/msg/string.hpp" @@ -105,6 +106,32 @@ struct TypeAdapter } }; +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::LargeMessage; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.size = source.size(); + std::memcpy(destination.data.data(), source.data(), source.size()); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination.resize(source.size); + std::memcpy(destination.data(), source.data.data(), source.size); + } +}; + } // namespace rclcpp /* @@ -152,33 +179,54 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { } } +using UseTakeSharedMethod = bool; +class TestPublisherFixture + : public TestPublisher, + public ::testing::WithParamInterface +{ +}; + /* * Testing that publisher sends type adapted types and ROS message types with intra proccess communications. */ -TEST_F( - TestPublisher, +TEST_P( + TestPublisherFixture, check_type_adapted_message_is_sent_and_received_intra_process) { using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; const std::string topic_name = "topic_name"; bool is_received; - auto callback = - [message_data, &is_received]( - const rclcpp::msg::String::ConstSharedPtr msg, - const rclcpp::MessageInfo & message_info - ) -> void - { - is_received = true; - ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); - }; - auto node = rclcpp::Node::make_shared( "test_intra_process", rclcpp::NodeOptions().use_intra_process_comms(true)); auto pub = node->create_publisher(topic_name, 10); - auto sub = node->create_subscription(topic_name, 1, callback); + rclcpp::Subscription::SharedPtr sub; + if (GetParam()) { + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String::ConstSharedPtr msg, + const rclcpp::MessageInfo & message_info + ) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + sub = node->create_subscription(topic_name, 1, callback); + } else { + auto callback_unique = + [message_data, &is_received]( + rclcpp::msg::String::UniquePtr msg, + const rclcpp::MessageInfo & message_info + ) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + sub = node->create_subscription(topic_name, 1, callback_unique); + } auto wait_for_message_to_be_received = [&is_received, &node]() { rclcpp::executors::SingleThreadedExecutor executor; @@ -239,6 +287,14 @@ TEST_F( } } +INSTANTIATE_TEST_SUITE_P( + TestPublisherFixtureWithParam, + TestPublisherFixture, + ::testing::Values( + true, // use take shared method + false // not use take shared method +)); + /* * Testing that publisher sends type adapted types and ROS message types with inter proccess communications. */ @@ -307,3 +363,45 @@ TEST_F(TestPublisher, check_type_adapted_message_is_sent_and_received) { assert_message_was_received(); } } + +TEST_F(TestPublisher, test_large_message_unique) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + auto message_data = std::make_unique(); + message_data->reserve(length); + std::fill(message_data->begin(), message_data->begin() + length, '#'); + pub->publish(std::move(message_data)); +} + +TEST_F(TestPublisher, test_large_message_constref) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + std::string message_data; + message_data.reserve(length); + std::fill(message_data.begin(), message_data.begin() + length, '#'); + pub->publish(message_data); +} diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 6b522d7ea2..ddd6b8db1c 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -301,24 +301,19 @@ TEST_F(TestQosEvent, add_to_wait_set) { { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_add_event, RCL_RET_OK); - EXPECT_NO_THROW(handler.add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(handler.add_to_wait_set(wait_set)); } { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_wait_set_add_event, RCL_RET_ERROR); - EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError); + EXPECT_THROW(handler.add_to_wait_set(wait_set), rclcpp::exceptions::RCLError); } } TEST_F(TestQosEvent, test_on_new_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); @@ -360,11 +355,6 @@ TEST_F(TestQosEvent, test_on_new_event_callback) TEST_F(TestQosEvent, test_invalid_on_new_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto pub = node->create_publisher(topic_name, 10); auto sub = node->create_subscription(topic_name, 10, message_callback); auto dummy_cb = [](size_t count_events) {(void)count_events;}; @@ -439,11 +429,6 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - std::atomic_size_t matched_count = 0; rclcpp::PublisherOptions pub_options; @@ -451,8 +436,10 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) auto pub = node->create_publisher( topic_name, 10, pub_options); - auto matched_event_callback = [&matched_count](size_t count) { + std::promise prom; + auto matched_event_callback = [&matched_count, &prom](size_t count) { matched_count += count; + prom.set_value(); }; pub->set_on_new_qos_event_callback(matched_event_callback, RCL_PUBLISHER_MATCHED); @@ -460,34 +447,32 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(node->get_node_base_interface()); - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto sub1 = node->create_subscription(topic_name, 10, message_callback); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(1)); { auto sub2 = node->create_subscription( topic_name, 10, message_callback); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(2)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(3)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); EXPECT_EQ(matched_count, static_cast(4)); } TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - std::atomic_size_t matched_count = 0; rclcpp::SubscriptionOptions sub_options; @@ -495,8 +480,10 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) auto sub = node->create_subscription( topic_name, 10, message_callback, sub_options); - auto matched_event_callback = [&matched_count](size_t count) { + std::promise prom; + auto matched_event_callback = [&matched_count, &prom](size_t count) { matched_count += count; + prom.set_value(); }; sub->set_on_new_qos_event_callback(matched_event_callback, RCL_SUBSCRIPTION_MATCHED); @@ -504,39 +491,44 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(node->get_node_base_interface()); - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10000); { auto pub1 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(1)); { auto pub2 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(2)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(3)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); EXPECT_EQ(matched_count, static_cast(4)); } TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) { rmw_matched_status_t matched_expected_result; + std::promise prom; rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [&matched_expected_result](rmw_matched_status_t & s) { + [&matched_expected_result, &prom](rmw_matched_status_t & s) { EXPECT_EQ(s.total_count, matched_expected_result.total_count); EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); EXPECT_EQ(s.current_count, matched_expected_result.current_count); EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + prom.set_value(); }; auto pub = node->create_publisher( @@ -551,11 +543,12 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) matched_expected_result.current_count = 1; matched_expected_result.current_count_change = 1; - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto sub = node->create_subscription(topic_name, 10, message_callback); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; // destroy a connected subscription matched_expected_result.total_count = 1; @@ -563,20 +556,22 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) matched_expected_result.current_count = 0; matched_expected_result.current_count_change = -1; } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); } TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) { rmw_matched_status_t matched_expected_result; + std::promise prom; rclcpp::SubscriptionOptions sub_options; sub_options.event_callbacks.matched_callback = - [&matched_expected_result](rmw_matched_status_t & s) { + [&matched_expected_result, &prom](rmw_matched_status_t & s) { EXPECT_EQ(s.total_count, matched_expected_result.total_count); EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); EXPECT_EQ(s.current_count, matched_expected_result.current_count); EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + prom.set_value(); }; auto sub = node->create_subscription( topic_name, 10, message_callback, sub_options); @@ -590,10 +585,11 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) matched_expected_result.current_count = 1; matched_expected_result.current_count_change = 1; - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto pub1 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; // destroy a connected publisher matched_expected_result.total_count = 1; @@ -601,5 +597,5 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) matched_expected_result.current_count = 0; matched_expected_result.current_count_change = -1; } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); } diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index d6608d59f6..b80789c853 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -14,14 +14,31 @@ #include +#include #include #include "rclcpp/rate.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +class TestRate : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + /* Basic tests for the Rate and WallRate classes. */ -TEST(TestRate, rate_basics) { +TEST_F(TestRate, rate_basics) { auto period = std::chrono::milliseconds(1000); auto offset = std::chrono::milliseconds(500); auto epsilon = std::chrono::milliseconds(100); @@ -29,8 +46,23 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); - EXPECT_EQ(period, r.period()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_FALSE(r.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + ASSERT_EQ(RCL_SYSTEM_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); auto delta = one - start; @@ -61,7 +93,7 @@ TEST(TestRate, rate_basics) { ASSERT_TRUE(epsilon > delta); } -TEST(TestRate, wall_rate_basics) { +TEST_F(TestRate, wall_rate_basics) { auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -69,8 +101,25 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); - EXPECT_EQ(period, r.period()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_TRUE(r.is_steady()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + ASSERT_EQ(RCL_STEADY_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); auto delta = one - start; @@ -101,21 +150,221 @@ TEST(TestRate, wall_rate_basics) { EXPECT_GT(epsilon, delta); } -TEST(TestRate, from_double) { +/* + Basic test for the deprecated GenericRate class. + */ +TEST_F(TestRate, generic_rate) { + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + { + auto start = std::chrono::system_clock::now(); + +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(period); + ASSERT_FALSE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_EQ(gr.get_type(), RCL_SYSTEM_TIME); + EXPECT_EQ(period, gr.period()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::system_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::system_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::system_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::system_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::system_clock::now(); + ASSERT_FALSE(gr.sleep()); + auto five = std::chrono::system_clock::now(); + delta = five - four; + ASSERT_TRUE(epsilon > delta); + } + + { + auto start = std::chrono::steady_clock::now(); + +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(period); + ASSERT_TRUE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_EQ(gr.get_type(), RCL_STEADY_TIME); + EXPECT_EQ(period, gr.period()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::steady_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::steady_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta + epsilon); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::steady_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::steady_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::steady_clock::now(); + ASSERT_FALSE(gr.sleep()); + auto five = std::chrono::steady_clock::now(); + delta = five - four; + EXPECT_GT(epsilon, delta); + } +} + +TEST_F(TestRate, from_double) { { - rclcpp::WallRate rate(1.0); - EXPECT_EQ(std::chrono::seconds(1), rate.period()); + rclcpp::Rate rate(1.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(1)), rate.period()); } { - rclcpp::WallRate rate(2.0); - EXPECT_EQ(std::chrono::milliseconds(500), rate.period()); + rclcpp::Rate rate(2.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); } { rclcpp::WallRate rate(0.5); - EXPECT_EQ(std::chrono::seconds(2), rate.period()); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(2)), rate.period()); } { rclcpp::WallRate rate(4.0); - EXPECT_EQ(std::chrono::milliseconds(250), rate.period()); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(250)), rate.period()); + } +} + +TEST_F(TestRate, clock_types) { + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_TRUE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); } } + +TEST_F(TestRate, incorrect_constuctor) { + // Constructor with 0-frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(0.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with negative frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(-1.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with 0-duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(0, 0)), + std::invalid_argument("period must be greater than 0")); + + // Constructor with negative duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(-1, 0)), + std::invalid_argument("period must be greater than 0")); +} diff --git a/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp index 07dfd8d584..0abd9b1a89 100644 --- a/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp +++ b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp @@ -22,7 +22,7 @@ #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" /* - Construtctor + * Construtctor */ TEST(TestRingBufferImplementation, constructor) { // Cannot create a buffer of size zero. @@ -37,10 +37,11 @@ TEST(TestRingBufferImplementation, constructor) { } /* - Basic usage - - insert data and check that it has data - - extract data - - overwrite old data writing over the buffer capacity + * Basic usage + * - insert data and check that it has data + * - get all data + * - extract data + * - overwrite old data writing over the buffer capacity */ TEST(TestRingBufferImplementation, basic_usage) { rclcpp::experimental::buffers::RingBufferImplementation rb(2); @@ -64,6 +65,12 @@ TEST(TestRingBufferImplementation, basic_usage) { rb.enqueue('d'); + const auto all_data_vec = rb.get_all_data(); + + EXPECT_EQ(2u, all_data_vec.size()); + EXPECT_EQ('c', all_data_vec[0]); + EXPECT_EQ('d', all_data_vec[1]); + EXPECT_EQ(true, rb.has_data()); EXPECT_EQ(true, rb.is_full()); @@ -79,3 +86,56 @@ TEST(TestRingBufferImplementation, basic_usage) { EXPECT_EQ(false, rb.has_data()); EXPECT_EQ(false, rb.is_full()); } + +/* + * Basic usage with unique_ptr + * - insert unique_ptr data and check that it has data + * - get all data + * - extract data + * - overwrite old data writing over the buffer capacity + */ +TEST(TestRingBufferImplementation, basic_usage_unique_ptr) { + rclcpp::experimental::buffers::RingBufferImplementation> rb(2); + + auto a = std::make_unique('a'); + auto b = std::make_unique('b'); + auto original_b_pointer = reinterpret_cast(b.get()); + auto c = std::make_unique('c'); + auto original_c_pointer = reinterpret_cast(c.get()); + + rb.enqueue(std::move(a)); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + rb.enqueue(std::move(b)); + rb.enqueue(std::move(c)); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + const auto all_data_vec = rb.get_all_data(); + + EXPECT_EQ(2u, all_data_vec.size()); + EXPECT_EQ('b', *all_data_vec[0]); + EXPECT_EQ('c', *all_data_vec[1]); + EXPECT_NE(original_b_pointer, reinterpret_cast(all_data_vec[0].get())); + EXPECT_NE(original_c_pointer, reinterpret_cast(all_data_vec[1].get())); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + auto uni_ptr = rb.dequeue(); + + EXPECT_EQ('b', *uni_ptr); + EXPECT_EQ(original_b_pointer, reinterpret_cast(uni_ptr.get())); + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + uni_ptr = rb.dequeue(); + + EXPECT_EQ('c', *uni_ptr); + EXPECT_EQ(original_c_pointer, reinterpret_cast(uni_ptr.get())); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} diff --git a/rclcpp/test/rclcpp/test_rosout_subscription.cpp b/rclcpp/test/rclcpp/test_rosout_subscription.cpp index ea761fdc0c..08786a3fcb 100644 --- a/rclcpp/test/rclcpp/test_rosout_subscription.cpp +++ b/rclcpp/test/rclcpp/test_rosout_subscription.cpp @@ -70,7 +70,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // before calling get_child of Logger { RCLCPP_INFO( - rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); @@ -83,7 +83,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // after calling get_child of Logger // 1. use child_logger directly { - RCLCPP_INFO(child_logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(child_logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -93,7 +93,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // 2. use rclcpp::get_logger { - RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + RCLCPP_INFO(rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -104,7 +104,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // `child_logger` is end of life, there is no sublogger { - RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + RCLCPP_INFO(rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); @@ -119,7 +119,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_parent_log) { rclcpp::Logger logger = this->node->get_logger(); ASSERT_EQ(logger.get_name(), logger_name); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -133,14 +133,14 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { this->rosout_msg_name = logger_name; rclcpp::Logger logger = this->node->get_logger(); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); received_msg_promise = {}; logger = this->node->get_logger().get_child("child1"); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); future = received_msg_promise.get_future(); return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -148,14 +148,14 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { received_msg_promise = {}; logger = this->node->get_logger().get_child("child2"); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); future = received_msg_promise.get_future(); return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); received_msg_promise = {}; this->rosout_msg_name = "ns.test_rosout_subscription.child2"; - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); future = received_msg_promise.get_future(); return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -171,10 +171,20 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild_hierarchy) { rclcpp::Logger grandchild_logger = this->node->get_logger().get_child("child").get_child("grandchild"); ASSERT_EQ(grandchild_logger.get_name(), logger_name); - RCLCPP_INFO(grandchild_logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(grandchild_logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); EXPECT_TRUE(future.get()); received_msg_promise = {}; } + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_node_rosout_disabled) { + rclcpp::NodeOptions options = rclcpp::NodeOptions().enable_rosout(false); + auto node = std::make_shared("my_node", options); + auto log_func = [&node] { + auto logger = node->get_logger().get_child("child"); + RCLCPP_INFO(logger, "test"); + }; + ASSERT_NO_THROW(log_func()); +} diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp index 0a020bb7c0..67a2d9b2be 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -145,11 +145,6 @@ TEST(TestSerializedMessage, reserve) { // Resize using reserve method serialized_msg.reserve(15); EXPECT_EQ(15u, serialized_msg.capacity()); - - // Use invalid value throws exception - EXPECT_THROW( - {serialized_msg.reserve(-1);}, - rclcpp::exceptions::RCLBadAlloc); } TEST(TestSerializedMessage, serialization) { diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index a3c361cfde..1a00ceb527 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -88,12 +88,11 @@ class TestServiceSub : public ::testing::Test Testing service construction and destruction. */ TEST_F(TestService, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; { - auto service = node->create_service("service", callback); + auto service = node->create_service("service", callback); EXPECT_NE(nullptr, service->get_service_handle()); const rclcpp::ServiceBase * const_service_base = service.get(); EXPECT_NE(nullptr, const_service_base->get_service_handle()); @@ -102,7 +101,8 @@ TEST_F(TestService, construction_and_destruction) { { ASSERT_THROW( { - auto service = node->create_service("invalid_service?", callback); + auto service = node->create_service( + "invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } } @@ -111,27 +111,27 @@ TEST_F(TestService, construction_and_destruction) { Testing service construction and destruction for subnodes. */ TEST_F(TestServiceSub, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; { - auto service = subnode->create_service("service", callback); + auto service = subnode->create_service( + "service", callback); EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service"); } { ASSERT_THROW( { - auto service = node->create_service("invalid_service?", callback); + auto service = node->create_service( + "invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } } TEST_F(TestService, construction_and_destruction_rcl_errors) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR); @@ -149,11 +149,10 @@ TEST_F(TestService, construction_and_destruction_rcl_errors) { /* Testing basic getters */ TEST_F(TestService, basic_public_getters) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; - auto service = node->create_service("service", callback); + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; + auto service = node->create_service("service", callback); EXPECT_STREQ(service->get_service_name(), "/ns/service"); std::shared_ptr service_handle = service->get_service_handle(); EXPECT_NE(nullptr, service_handle); @@ -189,9 +188,8 @@ TEST_F(TestService, basic_public_getters) { } TEST_F(TestService, take_request) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); { auto request_id = server->create_request_header(); @@ -217,9 +215,8 @@ TEST_F(TestService, take_request) { } TEST_F(TestService, send_response) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); { @@ -243,9 +240,9 @@ TEST_F(TestService, send_response) { Testing on_new_request callbacks. */ TEST_F(TestService, on_new_request_callback) { - auto server_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; + auto server_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; rclcpp::ServicesQoS service_qos; service_qos.keep_last(3); auto server = node->create_service( @@ -315,9 +312,8 @@ TEST_F(TestService, on_new_request_callback) { TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); RCLCPP_EXPECT_THROW_EQ( server->get_response_publisher_actual_qos(), @@ -327,9 +323,8 @@ TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); RCLCPP_EXPECT_THROW_EQ( server->get_request_subscription_actual_qos(), @@ -345,11 +340,10 @@ TEST_F(TestService, server_qos) { qos_profile.lifespan(duration); qos_profile.liveliness_lease_duration(duration); - auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; - auto server = node->create_service( - "service", callback, qos_profile); + auto server = node->create_service("service", callback, qos_profile); auto rs_qos = server->get_request_subscription_actual_qos(); auto rp_qos = server->get_response_publisher_actual_qos(); @@ -360,8 +354,6 @@ TEST_F(TestService, server_qos) { } TEST_F(TestService, server_qos_depth) { - using namespace std::literals::chrono_literals; - uint64_t server_cb_count_ = 0; auto server_callback = [&]( const test_msgs::srv::Empty::Request::SharedPtr, @@ -380,8 +372,8 @@ TEST_F(TestService, server_qos_depth) { ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); auto request = std::make_shared(); - using SharedFuture = rclcpp::Client::SharedFuture; - auto client_callback = [&request_result](SharedFuture future_response) { + auto client_callback = [&request_result]( + rclcpp::Client::SharedFuture future_response) { if (nullptr == future_response.get()) { request_result = ::testing::AssertionFailure() << "Future response was null"; } diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 6facada4dd..06f6f9785b 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include #include #include @@ -34,53 +36,7 @@ using namespace std::chrono_literals; class TestSubscription : public ::testing::Test { public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) - { - (void)msg; - } - - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("test_subscription", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -struct TestParameters -{ - TestParameters(rclcpp::QoS qos, std::string description) - : qos(qos), description(description) {} - rclcpp::QoS qos; - std::string description; -}; - -std::ostream & operator<<(std::ostream & out, const TestParameters & params) -{ - out << params.description; - return out; -} - -class TestSubscriptionInvalidIntraprocessQos - : public TestSubscription, - public ::testing::WithParamInterface -{}; - -class TestSubscriptionSub : public ::testing::Test -{ -public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -88,60 +44,20 @@ class TestSubscriptionSub : public ::testing::Test protected: static void SetUpTestCase() { + rclcpp::init(0, nullptr); } - void SetUp() - { - node = std::make_shared("test_subscription", "/ns"); - subnode = node->create_sub_node("sub_ns"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; - rclcpp::Node::SharedPtr subnode; -}; - -class SubscriptionClassNodeInheritance : public rclcpp::Node -{ -public: - SubscriptionClassNodeInheritance() - : Node("subscription_class_node_inheritance") - { - } - - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) - { - (void)msg; - } - - void CreateSubscription() + static void TearDownTestCase() { - auto callback = std::bind( - &SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1); - using test_msgs::msg::Empty; - auto sub = this->create_subscription("topic", 10, callback); + rclcpp::shutdown(); } -}; -class SubscriptionClass -{ -public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) { - (void)msg; + node_ = std::make_shared("test_subscription", "/ns", node_options); } - void CreateSubscription() - { - auto node = std::make_shared("test_subscription_member_callback", "/ns"); - auto callback = std::bind(&SubscriptionClass::OnMessage, this, std::placeholders::_1); - using test_msgs::msg::Empty; - auto sub = node->create_subscription("topic", 10, callback); - } + rclcpp::Node::SharedPtr node_; }; /* @@ -155,7 +71,7 @@ TEST_F(TestSubscription, construction_and_destruction) { }; { constexpr size_t depth = 10u; - auto sub = node->create_subscription("topic", depth, callback); + auto sub = node_->create_subscription("topic", depth, callback); EXPECT_NE(nullptr, sub->get_subscription_handle()); // Converting to base class was necessary for the compiler to choose the const version of @@ -172,40 +88,7 @@ TEST_F(TestSubscription, construction_and_destruction) { { ASSERT_THROW( { - auto sub = node->create_subscription("invalid_topic?", 10, callback); - }, rclcpp::exceptions::InvalidTopicNameError); - } -} - -/* - Testing subscription construction and destruction for subnodes. - */ -TEST_F(TestSubscriptionSub, construction_and_destruction) { - using test_msgs::msg::Empty; - auto callback = [](Empty::ConstSharedPtr msg) { - (void)msg; - }; - { - auto sub = subnode->create_subscription("topic", 1, callback); - EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); - } - - { - auto sub = subnode->create_subscription("/topic", 1, callback); - EXPECT_STREQ(sub->get_topic_name(), "/topic"); - } - - { - auto sub = subnode->create_subscription("~/topic", 1, callback); - std::string expected_topic_name = - std::string(node->get_namespace()) + "/" + node->get_name() + "/topic"; - EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); - } - - { - ASSERT_THROW( - { - auto sub = node->create_subscription("invalid_topic?", 1, callback); + auto sub = node_->create_subscription("invalid_topic?", 10, callback); }, rclcpp::exceptions::InvalidTopicNameError); } } @@ -218,31 +101,31 @@ TEST_F(TestSubscription, various_creation_signatures) { using test_msgs::msg::Empty; auto cb = [](test_msgs::msg::Empty::ConstSharedPtr) {}; { - auto sub = node->create_subscription("topic", 1, cb); + auto sub = node_->create_subscription("topic", 1, cb); (void)sub; } { - auto sub = node->create_subscription("topic", rclcpp::QoS(1), cb); + auto sub = node_->create_subscription("topic", rclcpp::QoS(1), cb); (void)sub; } { auto sub = - node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); (void)sub; } { auto sub = - node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); (void)sub; } { - auto sub = node->create_subscription( + auto sub = node_->create_subscription( "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } { auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, rclcpp::SubscriptionOptions()); + node_, "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } { @@ -250,40 +133,78 @@ TEST_F(TestSubscription, various_creation_signatures) { options.allocator = std::make_shared>(); EXPECT_NE(nullptr, options.get_allocator()); auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, options); + node_, "topic", 42, cb, options); (void)sub; } { rclcpp::SubscriptionOptionsBase options_base; rclcpp::SubscriptionOptionsWithAllocator> options(options_base); auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, options); + node_, "topic", 42, cb, options); (void)sub; } } +class SubscriptionClass final +{ +public: + void custom_create_subscription() + { + auto node = std::make_shared("test_subscription_member_callback", "/ns"); + auto callback = std::bind(&SubscriptionClass::on_message, this, std::placeholders::_1); + auto sub = node->create_subscription("topic", 10, callback); + } + +private: + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } +}; + +class SubscriptionClassNodeInheritance final : public rclcpp::Node +{ +public: + SubscriptionClassNodeInheritance() + : Node("subscription_class_node_inheritance") + { + } + + void custom_create_subscription() + { + auto callback = std::bind( + &SubscriptionClassNodeInheritance::on_message, this, std::placeholders::_1); + auto sub = this->create_subscription("topic", 10, callback); + } + +private: + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } +}; + /* Testing subscriptions using std::bind. */ TEST_F(TestSubscription, callback_bind) { initialize(); - using test_msgs::msg::Empty; { // Member callback for plain class SubscriptionClass subscription_object; - subscription_object.CreateSubscription(); + subscription_object.custom_create_subscription(); } { // Member callback for class inheriting from rclcpp::Node SubscriptionClassNodeInheritance subscription_object; - subscription_object.CreateSubscription(); + subscription_object.custom_create_subscription(); } { // Member callback for class inheriting from testing::Test // Regression test for https://github.com/ros2/rclcpp/issues/479 where the TEST_F GTest macro // was interfering with rclcpp's `function_traits`. - auto callback = std::bind(&TestSubscription::OnMessage, this, std::placeholders::_1); - auto sub = node->create_subscription("topic", 1, callback); + auto callback = std::bind(&TestSubscription::on_message, this, std::placeholders::_1); + auto sub = node_->create_subscription("topic", 1, callback); } } @@ -292,10 +213,9 @@ TEST_F(TestSubscription, callback_bind) { */ TEST_F(TestSubscription, take) { initialize(); - using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; { - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + auto sub = node_->create_subscription("~/test_take", 1, do_nothing); test_msgs::msg::Empty msg; rclcpp::MessageInfo msg_info; EXPECT_FALSE(sub->take(msg, msg_info)); @@ -303,23 +223,23 @@ TEST_F(TestSubscription, take) { { rclcpp::SubscriptionOptions so; so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + auto sub = node_->create_subscription("~/test_take", 1, do_nothing, so); rclcpp::PublisherOptions po; po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto pub = node->create_publisher("~/test_take", 1, po); + auto pub = node_->create_publisher("~/test_take", 1, po); { test_msgs::msg::Empty msg; pub->publish(msg); } test_msgs::msg::Empty msg; rclcpp::MessageInfo msg_info; - bool message_recieved = false; + bool message_received = false; auto start = std::chrono::steady_clock::now(); do { - message_recieved = sub->take(msg, msg_info); + message_received = sub->take(msg, msg_info); std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); } // TODO(wjwwood): figure out a good way to test the intra-process exclusion behavior. } @@ -329,10 +249,9 @@ TEST_F(TestSubscription, take) { */ TEST_F(TestSubscription, take_serialized) { initialize(); - using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; { - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + auto sub = node_->create_subscription("~/test_take", 1, do_nothing); std::shared_ptr msg = sub->create_serialized_message(); rclcpp::MessageInfo msg_info; EXPECT_FALSE(sub->take_serialized(*msg, msg_info)); @@ -340,23 +259,23 @@ TEST_F(TestSubscription, take_serialized) { { rclcpp::SubscriptionOptions so; so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + auto sub = node_->create_subscription("~/test_take", 1, do_nothing, so); rclcpp::PublisherOptions po; po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto pub = node->create_publisher("~/test_take", 1, po); + auto pub = node_->create_publisher("~/test_take", 1, po); { test_msgs::msg::Empty msg; pub->publish(msg); } std::shared_ptr msg = sub->create_serialized_message(); rclcpp::MessageInfo msg_info; - bool message_recieved = false; + bool message_received = false; auto start = std::chrono::steady_clock::now(); do { - message_recieved = sub->take_serialized(*msg, msg_info); + message_received = sub->take_serialized(*msg, msg_info); std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); } } @@ -368,7 +287,7 @@ TEST_F(TestSubscription, rcl_subscription_init_error) { // reset() is not needed for triggering exception, just to avoid an unused return value warning EXPECT_THROW( - node->create_subscription("topic", 10, callback).reset(), + node_->create_subscription("topic", 10, callback).reset(), rclcpp::exceptions::RCLError); } @@ -380,7 +299,7 @@ TEST_F(TestSubscription, rcl_subscription_fini_error) { // Cleanup just fails, no exception expected EXPECT_NO_THROW( - node->create_subscription("topic", 10, callback).reset()); + node_->create_subscription("topic", 10, callback).reset()); } TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { @@ -389,7 +308,7 @@ TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_actual_qos, nullptr); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); RCLCPP_EXPECT_THROW_EQ( sub->get_actual_qos(), std::runtime_error("failed to get qos settings: error not set")); } @@ -400,7 +319,7 @@ TEST_F(TestSubscription, rcl_take_type_erased_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_take, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); test_msgs::msg::Empty msg; rclcpp::MessageInfo message_info; @@ -413,7 +332,7 @@ TEST_F(TestSubscription, rcl_take_serialized_message_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_take_serialized_message, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); rclcpp::SerializedMessage msg; rclcpp::MessageInfo message_info; @@ -426,14 +345,14 @@ TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_publisher_count, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); EXPECT_THROW(sub->get_publisher_count(), rclcpp::exceptions::RCLError); } TEST_F(TestSubscription, handle_loaned_message) { initialize(); auto callback = [](std::shared_ptr) {}; - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); test_msgs::msg::Empty msg; rclcpp::MessageInfo message_info; @@ -448,13 +367,13 @@ TEST_F(TestSubscription, on_new_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 10, do_nothing); + auto sub = node_->create_subscription("~/test_take", 10, do_nothing); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; sub->set_on_new_message_callback(increase_c1_cb); - auto pub = node->create_publisher("~/test_take", 3); + auto pub = node_->create_publisher("~/test_take", 3); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -518,13 +437,13 @@ TEST_F(TestSubscription, on_new_intra_process_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 10, do_nothing); + auto sub = node_->create_subscription("~/test_take", 10, do_nothing); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; sub->set_on_new_intra_process_message_callback(increase_c1_cb); - auto pub = node->create_publisher("~/test_take", 1); + auto pub = node_->create_publisher("~/test_take", 1); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -580,21 +499,146 @@ TEST_F(TestSubscription, on_new_intra_process_message_callback) { EXPECT_THROW(sub->set_on_new_intra_process_message_callback(invalid_cb), std::invalid_argument); } +TEST_F(TestSubscription, get_network_flow_endpoints_errors) { + initialize(); + const rclcpp::QoS subscription_qos(1); + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + auto subscription = node_->create_subscription( + "topic", subscription_qos, subscription_callback); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_ERROR); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_OK); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); + EXPECT_NO_THROW(subscription->get_network_flow_endpoints()); + } +} + +class TestSubscriptionSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node_ = std::make_shared("test_subscription", "/ns"); + subnode_ = node_->create_sub_node("sub_ns"); + } + + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr subnode_; +}; + +/* + Testing subscription construction and destruction for subnodes. + */ +TEST_F(TestSubscriptionSub, construction_and_destruction) { + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + { + auto sub = subnode_->create_subscription("topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); + } + + { + auto sub = subnode_->create_subscription("/topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/topic"); + } + + { + auto sub = subnode_->create_subscription("~/topic", 1, callback); + std::string expected_topic_name = + std::string(node_->get_namespace()) + "/" + node_->get_name() + "/topic"; + EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); + } + + { + ASSERT_THROW( + { + auto sub = node_->create_subscription("invalid_topic?", 1, callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +struct TestParameters final +{ + TestParameters(rclcpp::QoS qos, std::string description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestSubscriptionInvalidIntraprocessQos + : public TestSubscription, + public ::testing::WithParamInterface +{}; + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(1); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + + return parameters; +} + +INSTANTIATE_TEST_SUITE_P( + TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); + /* Testing subscription with intraprocess enabled and invalid QoS */ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); rclcpp::QoS qos = GetParam().qos; - using test_msgs::msg::Empty; { auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, + &TestSubscriptionInvalidIntraprocessQos::on_message, this, std::placeholders::_1); ASSERT_THROW( - {auto subscription = node->create_subscription( + {auto subscription = node_->create_subscription( "topic", qos, callback);}, @@ -612,71 +656,15 @@ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws_intrapro initialize(); rclcpp::QoS qos = GetParam().qos; auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, + &TestSubscriptionInvalidIntraprocessQos::on_message, this, std::placeholders::_1); RCLCPP_EXPECT_THROW_EQ( - {auto subscription = node->create_subscription( + {auto subscription = node_->create_subscription( "topic", qos, callback, options);}, std::runtime_error("Unrecognized IntraProcessSetting value")); } - -static std::vector invalid_qos_profiles() -{ - std::vector parameters; - - parameters.reserve(3); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepAll()), - "keep_all_qos")); - - return parameters; -} - -INSTANTIATE_TEST_SUITE_P( - TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, - ::testing::ValuesIn(invalid_qos_profiles()), - ::testing::PrintToStringParamName()); - -TEST_F(TestSubscription, get_network_flow_endpoints_errors) { - initialize(); - const rclcpp::QoS subscription_qos(1); - auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { - (void)msg; - }; - auto subscription = node->create_subscription( - "topic", subscription_qos, subscription_callback); - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_ERROR); - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); - EXPECT_THROW( - subscription->get_network_flow_endpoints(), - rclcpp::exceptions::RCLError); - } - { - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); - EXPECT_THROW( - subscription->get_network_flow_endpoints(), - rclcpp::exceptions::RCLError); - } - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_OK); - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); - EXPECT_NO_THROW(subscription->get_network_flow_endpoints()); - } -} diff --git a/rclcpp/test/rclcpp/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp index c4cfb6b4c4..d1122333bc 100644 --- a/rclcpp/test/rclcpp/test_subscription_options.cpp +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -60,14 +60,17 @@ TEST_F(TestSubscriptionOptions, topic_statistics_options_default_and_set) { EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::NodeDefault); EXPECT_EQ(options.topic_stats_options.publish_topic, defaultPublishTopic); EXPECT_EQ(options.topic_stats_options.publish_period, 1s); + EXPECT_EQ(options.topic_stats_options.qos, rclcpp::SystemDefaultsQoS()); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; options.topic_stats_options.publish_topic = "topic_statistics"; options.topic_stats_options.publish_period = 5min; + options.topic_stats_options.qos = rclcpp::BestAvailableQoS(); EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::Enable); EXPECT_EQ(options.topic_stats_options.publish_topic, "topic_statistics"); EXPECT_EQ(options.topic_stats_options.publish_period, 5min); + EXPECT_EQ(options.topic_stats_options.qos, rclcpp::BestAvailableQoS()); } TEST_F(TestSubscriptionOptions, topic_statistics_options_node_default_mode) { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index f3969d3886..7d656fb9c4 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -363,10 +363,27 @@ TEST_F(TestTime, seconds) { } TEST_F(TestTime, test_max) { - const rclcpp::Time time_max = rclcpp::Time::max(); - const rclcpp::Time max_time(std::numeric_limits::max(), 999999999); - EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds()); - EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds()); + // Same clock types + for (rcl_clock_type_t type = RCL_ROS_TIME; + type != RCL_STEADY_TIME; type = static_cast(type + 1)) + { + const rclcpp::Time time_max = rclcpp::Time::max(type); + const rclcpp::Time max_time(std::numeric_limits::max(), 999999999, type); + EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds()); + EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds()); + } + // Different clock types + { + const rclcpp::Time time_max = rclcpp::Time::max(RCL_ROS_TIME); + const rclcpp::Time max_time(std::numeric_limits::max(), 999999999, RCL_STEADY_TIME); + EXPECT_ANY_THROW((void)(time_max == max_time)); + EXPECT_ANY_THROW((void)(time_max != max_time)); + EXPECT_ANY_THROW((void)(time_max <= max_time)); + EXPECT_ANY_THROW((void)(time_max >= max_time)); + EXPECT_ANY_THROW((void)(time_max < max_time)); + EXPECT_ANY_THROW((void)(time_max > max_time)); + EXPECT_ANY_THROW((void)(time_max - max_time)); + } } TEST_F(TestTime, test_constructor_from_rcl_time_point) { diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 26d7cb2192..1437e47126 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -305,7 +305,7 @@ TEST_F(TestTimeSource, clock) { trigger_clock_changes(node, ros_clock, false); - // Even now that we've recieved a message, ROS time should still not be active since the + // Even now that we've received a message, ROS time should still not be active since the // parameter has not been explicitly set. EXPECT_FALSE(ros_clock->ros_time_is_active()); diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 59a1218519..6d26bc54b8 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -73,6 +73,20 @@ class TestTimer : public ::testing::TestWithParam EXPECT_FALSE(timer->is_steady()); break; } + timer_without_autostart = test_node->create_wall_timer( + 100ms, + [this]() -> void + { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); + } + // prevent any tests running timer from blocking + this->executor->cancel(); + }, nullptr, false); + EXPECT_TRUE(timer_without_autostart->is_steady()); + executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -93,6 +107,7 @@ class TestTimer : public ::testing::TestWithParam std::atomic cancel_timer; rclcpp::Node::SharedPtr test_node; std::shared_ptr timer; + std::shared_ptr timer_without_autostart; std::shared_ptr executor; }; @@ -270,20 +285,14 @@ TEST_P(TestTimer, test_failures_with_exceptions) std::shared_ptr timer_to_test_destructor; // Test destructor failure, just logs a msg auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); - EXPECT_NO_THROW( - { - switch (timer_type) { - case TimerType::WALL_TIMER: - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); - break; - case TimerType::GENERIC_TIMER: - timer_to_test_destructor = - test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); - break; - } - timer_to_test_destructor.reset(); - }); + if (timer_type == TimerType::WALL_TIMER) { + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + } else { + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + } + timer_to_test_destructor.reset(); } { auto mock = mocking_utils::patch_and_return( @@ -334,3 +343,18 @@ INSTANTIATE_TEST_SUITE_P( return std::string("unknown"); } ); + +/// Simple test of a timer without autostart +TEST_P(TestTimer, test_timer_without_autostart) +{ + EXPECT_TRUE(timer_without_autostart->is_canceled()); + EXPECT_EQ( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + // Reset to change start timer + timer_without_autostart->reset(); + EXPECT_LE( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer_without_autostart->is_canceled()); +} diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp index 635ec322c0..90d321c188 100644 --- a/rclcpp/test/rclcpp/test_timers_manager.cpp +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -65,8 +66,10 @@ TEST_F(TestTimersManager, empty_manager) TEST_F(TestTimersManager, add_run_remove_timer) { size_t t_runs = 0; + std::chrono::milliseconds timer_period(10); + auto t = TimerT::make_shared( - 10ms, + timer_period, [&t_runs]() { t_runs++; }, @@ -79,7 +82,7 @@ TEST_F(TestTimersManager, add_run_remove_timer) timers_manager->add_timer(t); // Sleep for more 3 times the timer period - std::this_thread::sleep_for(30ms); + std::this_thread::sleep_for(3 * timer_period); // The timer is executed only once, even if we slept 3 times the period execute_all_ready_timers(timers_manager); @@ -191,67 +194,6 @@ TEST_F(TestTimersManager, head_not_ready) EXPECT_EQ(0u, t_runs); } -TEST_F(TestTimersManager, timers_order) -{ - auto timers_manager = std::make_shared( - rclcpp::contexts::get_global_default_context()); - - size_t t1_runs = 0; - auto t1 = TimerT::make_shared( - 10ms, - [&t1_runs]() { - t1_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - size_t t2_runs = 0; - auto t2 = TimerT::make_shared( - 30ms, - [&t2_runs]() { - t2_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - size_t t3_runs = 0; - auto t3 = TimerT::make_shared( - 100ms, - [&t3_runs]() { - t3_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - // Add timers in a random order - timers_manager->add_timer(t2); - timers_manager->add_timer(t3); - timers_manager->add_timer(t1); - - std::this_thread::sleep_for(10ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(1u, t1_runs); - EXPECT_EQ(0u, t2_runs); - EXPECT_EQ(0u, t3_runs); - - std::this_thread::sleep_for(30ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(2u, t1_runs); - EXPECT_EQ(1u, t2_runs); - EXPECT_EQ(0u, t3_runs); - - std::this_thread::sleep_for(100ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(3u, t1_runs); - EXPECT_EQ(2u, t2_runs); - EXPECT_EQ(1u, t3_runs); - - timers_manager->remove_timer(t1); - - std::this_thread::sleep_for(30ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(3u, t1_runs); - EXPECT_EQ(3u, t2_runs); - EXPECT_EQ(1u, t3_runs); -} - TEST_F(TestTimersManager, start_stop_timers_thread) { auto timers_manager = std::make_shared( @@ -274,7 +216,7 @@ TEST_F(TestTimersManager, timers_thread) auto timers_manager = std::make_shared( rclcpp::contexts::get_global_default_context()); - size_t t1_runs = 0; + int t1_runs = 0; auto t1 = TimerT::make_shared( 1ms, [&t1_runs]() { @@ -282,7 +224,7 @@ TEST_F(TestTimersManager, timers_thread) }, rclcpp::contexts::get_global_default_context()); - size_t t2_runs = 0; + int t2_runs = 0; auto t2 = TimerT::make_shared( 1ms, [&t2_runs]() { @@ -296,12 +238,12 @@ TEST_F(TestTimersManager, timers_thread) // Run timers thread for a while timers_manager->start(); - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(50ms); timers_manager->stop(); EXPECT_LT(1u, t1_runs); EXPECT_LT(1u, t2_runs); - EXPECT_EQ(t1_runs, t2_runs); + EXPECT_LE(std::abs(t1_runs - t2_runs), 1); } TEST_F(TestTimersManager, destructor) @@ -365,13 +307,13 @@ TEST_F(TestTimersManager, add_remove_while_thread_running) timers_manager->start(); // After a while remove t1 and add t2 - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(50ms); timers_manager->remove_timer(t1); size_t tmp_t1 = t1_runs; timers_manager->add_timer(t2); // Wait some more time and then stop - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(50ms); timers_manager->stop(); // t1 has stopped running @@ -417,3 +359,81 @@ TEST_F(TestTimersManager, infinite_loop) EXPECT_LT(0u, t1_runs); EXPECT_LT(0u, t2_runs); } + +// Validate that cancelling one timer yields no change in behavior for other +// timers. +TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + std::atomic t1_runs = 0; + const size_t cancel_iter = 5; + std::shared_ptr t1; + // After a while cancel t1. Don't remove it though. + // Simulates typical usage in a Node where a timer is cancelled but not removed, + // since typical users aren't going to mess around with the timer manager. + t1 = TimerT::make_shared( + 1ms, + [&t1_runs, &t1, cancel_iter]() { + t1_runs++; + if (t1_runs == cancel_iter) { + t1->cancel(); + } + }, + rclcpp::contexts::get_global_default_context()); + + std::atomic t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + // Start timers thread + timers_manager->start(); + + // Wait for t1 to be canceled + auto loop_start_time = std::chrono::high_resolution_clock::now(); + while (!t1->is_canceled()) { + auto now = std::chrono::high_resolution_clock::now(); + if (now - loop_start_time >= std::chrono::seconds(30)) { + FAIL() << "timeout waiting for t1 to be canceled"; + break; + } + std::this_thread::sleep_for(3ms); + } + + EXPECT_TRUE(t1->is_canceled()); + EXPECT_FALSE(t2->is_canceled()); + EXPECT_EQ(t1_runs, cancel_iter); + + // Verify that t2 is still being invoked + const size_t start_t2_runs = t2_runs; + const size_t num_t2_extra_runs = 6; + loop_start_time = std::chrono::high_resolution_clock::now(); + while (t2_runs < start_t2_runs + num_t2_extra_runs) { + auto now = std::chrono::high_resolution_clock::now(); + if (now - loop_start_time >= std::chrono::seconds(30)) { + FAIL() << "timeout waiting for t2 to do some runs"; + break; + } + std::this_thread::sleep_for(3ms); + } + + EXPECT_TRUE(t1->is_canceled()); + EXPECT_FALSE(t2->is_canceled()); + // t1 hasn't run since before + EXPECT_EQ(t1_runs, cancel_iter); + // t2 has run the expected additional number of times + EXPECT_GE(t2_runs, start_t2_runs + num_t2_extra_runs); + // the t2 runs are strictly more than the t1 runs + EXPECT_GT(t2_runs, t1_runs); + + timers_manager->stop(); +} diff --git a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp index 8cdcfc19c0..2117b89455 100644 --- a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp +++ b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp @@ -50,7 +50,7 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_legacy_library) { try { auto library = rclcpp::get_typesupport_library( "test_msgs/BasicTypes", "rosidl_typesupport_cpp"); - auto string_typesupport = rclcpp::get_typesupport_handle( + auto string_typesupport = rclcpp::get_message_typesupport_handle( "test_msgs/BasicTypes", "rosidl_typesupport_cpp", *library); EXPECT_THAT( @@ -65,7 +65,7 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { try { auto library = rclcpp::get_typesupport_library( "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp"); - auto string_typesupport = rclcpp::get_typesupport_handle( + auto string_typesupport = rclcpp::get_message_typesupport_handle( "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp", *library); EXPECT_THAT( @@ -75,3 +75,52 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { FAIL() << e.what(); } } + +TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_legacy_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/Empty", "rosidl_typesupport_cpp"); + auto empty_typesupport = rclcpp::get_service_typesupport_handle( + "test_msgs/Empty", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(empty_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/srv/Empty", "rosidl_typesupport_cpp"); + auto empty_typesupport = rclcpp::get_service_typesupport_handle( + "test_msgs/srv/Empty", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(empty_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, test_throw_exception_with_invalid_type) { + // message + std::string invalid_type = "test_msgs/msg/InvalidType"; + auto library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp"); + EXPECT_THROW( + rclcpp::get_message_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); + EXPECT_THROW( + rclcpp::get_service_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); + + // service + invalid_type = "test_msgs/srv/InvalidType"; + library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp"); + EXPECT_THROW( + rclcpp::get_service_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index ce6887c631..9166272207 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -22,12 +21,12 @@ #include #include #include +#include #include #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp/create_publisher.hpp" -#include "rclcpp/msg/message_with_header.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" @@ -36,10 +35,10 @@ #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "statistics_msgs/msg/metrics_message.hpp" -#include "statistics_msgs/msg/statistic_data_point.hpp" #include "statistics_msgs/msg/statistic_data_type.hpp" #include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/strings.hpp" #include "test_topic_stats_utils.hpp" @@ -67,7 +66,6 @@ constexpr const std::chrono::seconds kUnstableMessageAgeWindowDuration{ constexpr const std::chrono::seconds kUnstableMessageAgeOffset{std::chrono::seconds{1}}; } // namespace -using rclcpp::msg::MessageWithHeader; using test_msgs::msg::Empty; using rclcpp::topic_statistics::SubscriptionTopicStatistics; using statistics_msgs::msg::MetricsMessage; @@ -76,114 +74,73 @@ using statistics_msgs::msg::StatisticDataType; using libstatistics_collector::moving_average_statistics::StatisticData; /** - * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. - * \tparam CallbackMessageT + * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. */ -template -class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics +class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics { public: TestSubscriptionTopicStatistics( const std::string & node_name, rclcpp::Publisher::SharedPtr publisher) - : SubscriptionTopicStatistics(node_name, publisher) + : SubscriptionTopicStatistics(node_name, std::move(publisher)) { } - virtual ~TestSubscriptionTopicStatistics() = default; + ~TestSubscriptionTopicStatistics() override = default; /// Exposed for testing - std::vector get_current_collector_data() const - { - return SubscriptionTopicStatistics::get_current_collector_data(); - } + using SubscriptionTopicStatistics::get_current_collector_data; }; /** - * Empty publisher node: used to publish empty messages + * PublisherNode wrapper: used to create publisher node */ -class EmptyPublisher : public rclcpp::Node +template +class PublisherNode : public rclcpp::Node { public: - EmptyPublisher( + PublisherNode( const std::string & name, const std::string & topic, const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) : Node(name) { - publisher_ = create_publisher(topic, 10); + publisher_ = create_publisher(topic, 10); publish_timer_ = this->create_wall_timer( publish_period, [this]() { this->publish_message(); }); } - virtual ~EmptyPublisher() = default; + ~PublisherNode() override = default; private: void publish_message() { - auto msg = Empty{}; + auto msg = MessageT{}; publisher_->publish(msg); } - rclcpp::Publisher::SharedPtr publisher_; + typename rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; }; /** - * MessageWithHeader publisher node: used to publish MessageWithHeader with `header` value set - */ -class MessageWithHeaderPublisher : public rclcpp::Node -{ -public: - MessageWithHeaderPublisher( - const std::string & name, const std::string & topic, - const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) - : Node(name) - { - publisher_ = create_publisher(topic, 10); - publish_timer_ = this->create_wall_timer( - publish_period, [this]() { - this->publish_message(); - }); - uniform_dist_ = std::uniform_int_distribution{1000000, 100000000}; - } - - virtual ~MessageWithHeaderPublisher() = default; - -private: - void publish_message() - { - std::random_device rd; - std::mt19937 gen{rd()}; - uint32_t d = uniform_dist_(gen); - auto msg = MessageWithHeader{}; - // Subtract ~1 second (add some noise for a non-zero standard deviation) - // so the received message age calculation is always > 0 - msg.header.stamp = this->now() - rclcpp::Duration{1, d}; - publisher_->publish(msg); - } - - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr publish_timer_; - std::uniform_int_distribution uniform_dist_; -}; - -/** - * TransitionMessageStamp publisher node : used to publish MessageWithHeader with `header` value set + * TransitionMessageStamp publisher emulator node : used to emulate publishing messages by + * directly calling rclcpp::Subscription::handle_message(msg_shared_ptr, message_info). * The message age results change during the test. */ - -class TransitionMessageStampPublisher : public rclcpp::Node +template +class TransitionMessageStampPublisherEmulator : public rclcpp::Node { public: - TransitionMessageStampPublisher( - const std::string & name, const std::string & topic, + TransitionMessageStampPublisherEmulator( + const std::string & name, const std::chrono::seconds transition_duration, const std::chrono::seconds message_age_offset, + typename rclcpp::Subscription::SharedPtr subscription, const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) - : Node(name), transition_duration_(transition_duration), message_age_offset_(message_age_offset) + : Node(name), transition_duration_(transition_duration), message_age_offset_(message_age_offset), + subscription_(std::move(subscription)) { - publisher_ = create_publisher(topic, 10); publish_timer_ = this->create_wall_timer(publish_period, [this]() {this->publish_message();}); start_time_ = this->now(); } @@ -191,84 +148,66 @@ class TransitionMessageStampPublisher : public rclcpp::Node private: void publish_message() { - auto msg = MessageWithHeader{}; + std::shared_ptr msg_shared_ptr = std::make_shared(); + rmw_message_info_t rmw_message_info = rmw_get_zero_initialized_message_info(); + auto now = this->now(); auto elapsed_time = now - start_time_; if (elapsed_time < transition_duration_) { // Apply only to the topic statistics in the first half // Subtract offset so message_age is always >= offset. - msg.header.stamp = now - message_age_offset_; + rmw_message_info.source_timestamp = (now - message_age_offset_).nanoseconds(); } else { - msg.header.stamp = now; + rmw_message_info.source_timestamp = now.nanoseconds(); } - publisher_->publish(msg); + rclcpp::MessageInfo message_info{rmw_message_info}; + subscription_->handle_message(msg_shared_ptr, message_info); } std::chrono::seconds transition_duration_; std::chrono::seconds message_age_offset_; + typename rclcpp::Subscription::SharedPtr subscription_; rclcpp::Time start_time_; - - rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; }; /** - * Empty subscriber node: used to create subscriber topic statistics requirements + * Message subscriber node: used to create subscriber with enabled topic statistics collectors + * */ -class EmptySubscriber : public rclcpp::Node +template +class SubscriberWithTopicStatistics : public rclcpp::Node { public: - EmptySubscriber(const std::string & name, const std::string & topic) + SubscriberWithTopicStatistics( + const std::string & name, const std::string & topic, + std::chrono::milliseconds publish_period = defaultStatisticsPublishPeriod) : Node(name) { - // manually enable topic statistics via options + // Manually enable topic statistics via options auto options = rclcpp::SubscriptionOptions(); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_period = publish_period; - auto callback = [](Empty::UniquePtr msg) { + auto callback = [](typename MessageT::UniquePtr msg) { (void) msg; }; - subscription_ = create_subscription>( + subscription_ = create_subscription>( topic, rclcpp::QoS(rclcpp::KeepAll()), callback, options); } - virtual ~EmptySubscriber() = default; - -private: - rclcpp::Subscription::SharedPtr subscription_; -}; + ~SubscriberWithTopicStatistics() override = default; -/** - * MessageWithHeader subscriber node: used to create subscriber topic statistics requirements - */ -class MessageWithHeaderSubscriber : public rclcpp::Node -{ -public: - MessageWithHeaderSubscriber(const std::string & name, const std::string & topic) - : Node(name) + typename rclcpp::Subscription::SharedPtr get_subscription() { - // manually enable topic statistics via options - auto options = rclcpp::SubscriptionOptions(); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - options.topic_stats_options.publish_period = defaultStatisticsPublishPeriod; - - auto callback = [](MessageWithHeader::UniquePtr msg) { - (void) msg; - }; - subscription_ = create_subscription>( - topic, - rclcpp::QoS(rclcpp::KeepAll()), - callback, - options); + return subscription_; } - virtual ~MessageWithHeaderSubscriber() = default; private: - rclcpp::Subscription::SharedPtr subscription_; + typename rclcpp::Subscription::SharedPtr subscription_; }; /** @@ -277,43 +216,17 @@ class MessageWithHeaderSubscriber : public rclcpp::Node class TestSubscriptionTopicStatisticsFixture : public ::testing::Test { protected: - void SetUp() + void SetUp() override { rclcpp::init(0 /* argc */, nullptr /* argv */); } - void TearDown() + void TearDown() override { rclcpp::shutdown(); } }; -/** - * Check if a received statistics message is empty (no data was observed) - * \param message_to_check - */ -void check_if_statistics_message_is_empty(const MetricsMessage & message_to_check) -{ - for (const auto & stats_point : message_to_check.statistics) { - const auto type = stats_point.data_type; - switch (type) { - case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: - EXPECT_EQ(0, stats_point.data) << "unexpected sample count" << stats_point.data; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_TRUE(std::isnan(stats_point.data)) << "unexpected value" << stats_point.data << - " for type:" << type; - break; - default: - FAIL() << "received unknown statistics type: " << std::dec << - static_cast(type); - } - } -} - /** * Check if a received statistics message observed data and contains some calculation * \param message_to_check @@ -348,28 +261,13 @@ void check_if_statistic_message_is_populated(const MetricsMessage & message_to_c /** * Test an invalid argument is thrown for a bad input publish period. */ -TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) +TEST_F(TestSubscriptionTopicStatisticsFixture, test_invalid_publish_period) { - rclcpp::init(0 /* argc */, nullptr /* argv */); - - auto node = std::make_shared("test_period_node"); - - auto options = rclcpp::SubscriptionOptions(); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - options.topic_stats_options.publish_period = std::chrono::milliseconds(0); - - auto callback = [](Empty::UniquePtr msg) { - (void) msg; - }; - ASSERT_THROW( - (node->create_subscription>( - "should_throw_invalid_arg", - rclcpp::QoS(rclcpp::KeepAll()), - callback, - options)), std::invalid_argument); - - rclcpp::shutdown(); + SubscriberWithTopicStatistics( + "test_period_node", "should_throw_invalid_arg", std::chrono::milliseconds(0) + ), + std::invalid_argument); } /** @@ -378,7 +276,7 @@ TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) */ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { - auto empty_subscriber = std::make_shared( + auto empty_subscriber = std::make_shared>( kTestSubNodeName, kTestSubStatsEmptyTopic); @@ -389,7 +287,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) 10); // Construct a separate instance - auto sub_topic_stats = std::make_unique>( + auto sub_topic_stats = std::make_unique( empty_subscriber->get_name(), topic_stats_publisher); @@ -410,7 +308,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) { // Create an empty publisher - auto empty_publisher = std::make_shared( + auto empty_publisher = std::make_shared>( kTestPubNodeName, kTestSubStatsEmptyTopic); // empty_subscriber has a topic statistics instance as part of its subscription @@ -422,7 +320,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no "/statistics", kNumExpectedMessages); - auto empty_subscriber = std::make_shared( + auto empty_subscriber = std::make_shared>( kTestSubNodeName, kTestSubStatsEmptyTopic); @@ -432,74 +330,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no ex.add_node(empty_subscriber); // Spin and get future - ex.spin_until_future_complete( - statistics_listener->GetFuture(), - kTestTimeout); - - // Compare message counts, sample count should be the same as published and received count - EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived()); - - // Check the received message total count - const auto received_messages = statistics_listener->GetReceivedMessages(); - EXPECT_EQ(kNumExpectedMessages, received_messages.size()); - - // check the type of statistics that were received and their counts - uint64_t message_age_count{0}; - uint64_t message_period_count{0}; - - std::set received_metrics; - for (const auto & msg : received_messages) { - if (msg.metrics_source == "message_age") { - message_age_count++; - } - if (msg.metrics_source == "message_period") { - message_period_count++; - } - } - EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); - EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); - - // Check the collected statistics for message period. - // Message age statistics will not be calculated because Empty messages - // don't have a `header` with timestamp. This means that we expect to receive a `message_age` - // and `message_period` message for each empty message published. - for (const auto & msg : received_messages) { - if (msg.metrics_source == kMessageAgeSourceLabel) { - check_if_statistics_message_is_empty(msg); - } else if (msg.metrics_source == kMessagePeriodSourceLabel) { - check_if_statistic_message_is_populated(msg); - } - } -} - -TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) -{ - // Create a MessageWithHeader publisher - auto msg_with_header_publisher = std::make_shared( - kTestPubNodeName, - kTestSubStatsTopic); - // empty_subscriber has a topic statistics instance as part of its subscription - // this will listen to and generate statistics for the empty message - - // Create a listener for topic statistics messages - auto statistics_listener = std::make_shared( - "test_receive_stats_for_message_with_header", - "/statistics", - kNumExpectedMessages); - - auto msg_with_header_subscriber = std::make_shared( - kTestSubNodeName, - kTestSubStatsTopic); - - rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(msg_with_header_publisher); - ex.add_node(statistics_listener); - ex.add_node(msg_with_header_subscriber); - - // Spin and get future - ex.spin_until_future_complete( - statistics_listener->GetFuture(), - kTestTimeout); + ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); // Compare message counts, sample count should be the same as published and received count EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived()); @@ -524,6 +355,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); + // Check the collected statistics for message period. for (const auto & msg : received_messages) { check_if_statistic_message_is_populated(msg); } @@ -531,23 +363,27 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_include_window_reset) { - // Create a MessageWithHeader publisher - auto msg_with_header_publisher = std::make_shared( - kTestPubNodeName, kTestSubStatsTopic, kUnstableMessageAgeWindowDuration, - kUnstableMessageAgeOffset); - - // msg_with_header_subscriber has a topic statistics instance as part of its + // msg_subscriber_with_topic_statistics has a topic statistics instance as part of its // subscription this will listen to and generate statistics - auto msg_with_header_subscriber = - std::make_shared(kTestSubNodeName, kTestSubStatsTopic); + auto msg_subscriber_with_topic_statistics = + std::make_shared>( + kTestSubNodeName, + kTestSubStatsTopic); + + // Create a message publisher + auto msg_publisher = + std::make_shared>( + kTestPubNodeName, kUnstableMessageAgeWindowDuration, + kUnstableMessageAgeOffset, msg_subscriber_with_topic_statistics->get_subscription()); + // Create a listener for topic statistics messages auto statistics_listener = std::make_shared( "test_receive_stats_include_window_reset", "/statistics", kNumExpectedMessages); rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(msg_with_header_publisher); + ex.add_node(msg_publisher); ex.add_node(statistics_listener); - ex.add_node(msg_with_header_subscriber); + ex.add_node(msg_subscriber_with_topic_statistics); // Spin and get future ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp index 9afb97536a..12bd2f884e 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -51,14 +51,14 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp index 470d0de361..55573cc11b 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -50,14 +50,14 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp index d217286da1..cd44918236 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -50,19 +50,19 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false), add_to_wait_set_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t &) override { if (!add_to_wait_set_) { throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); } } - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr & data) override {(void)data;} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp index 347714bbf7..7554f56270 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -50,14 +50,14 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 26f00ce559..b7b7a7e4c7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,68 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.0 (2024-03-28) +------------------- +* Do not generate the exception when action service response timeout. (`#2464 `_) + * Do not generate the exception when action service response timeout. + * address review comment. + --------- +* Modify rclcpp_action::GoalUUID hashing algorithm (`#2441 `_) + * Add unit tests for hashing rclcpp_action::GoalUUID's + * Use the FNV-1a hash algorithm for Goal UUID +* Various cleanups to deal with uncrustify 0.78. (`#2439 `_) + These should also work with uncrustify 0.72. +* Update quality declaration documents (`#2427 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Tomoya Fujita, mauropasse + +27.0.0 (2024-02-07) +------------------- + +26.0.0 (2024-01-24) +------------------- + +25.0.0 (2023-12-26) +------------------- +* Switch to target_link_libraries. (`#2374 `_) +* Contributors: Chris Lalancette + +24.0.0 (2023-11-06) +------------------- + +23.2.0 (2023-10-09) +------------------- + +23.1.0 (2023-10-04) +------------------- + +23.0.0 (2023-09-08) +------------------- +* Update API docs links in package READMEs (`#2302 `_) +* fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks (`#2267 `_) +* Contributors: Christophe Bedard, jmachowinski + +22.2.0 (2023-09-07) +------------------- +* Correct the position of a comment. (`#2290 `_) +* Fix a typo in a comment. (`#2283 `_) +* doc fix: call `canceled` only after goal state is in canceling. (`#2266 `_) +* Contributors: Chris Lalancette, Jiaqi Li, Tomoya Fujita + +22.1.0 (2023-08-21) +------------------- + +22.0.0 (2023-07-11) +------------------- + +21.3.0 (2023-06-12) +------------------- + +21.2.0 (2023-06-07) +------------------- + +21.1.1 (2023-05-11) +------------------- + 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 785ed0e5e1..647a820969 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -5,6 +5,7 @@ project(rclcpp_action) find_package(ament_cmake_ros REQUIRED) find_package(action_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl REQUIRED) find_package(rcl_action REQUIRED) find_package(rcpputils REQUIRED) find_package(rosidl_runtime_c REQUIRED) @@ -21,28 +22,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") ) endif() -set(${PROJECT_NAME}_SRCS +add_library(${PROJECT_NAME} src/client.cpp src/qos.cpp src/server.cpp src/server_goal_handle.cpp src/types.cpp ) - -add_library(${PROJECT_NAME} - ${${PROJECT_NAME}_SRCS}) - target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") - -ament_target_dependencies(${PROJECT_NAME} - "action_msgs" - "rcl_action" - "rclcpp" - "rcpputils" - "rosidl_runtime_c" +target_link_libraries(${PROJECT_NAME} PUBLIC + ${action_msgs_TARGETS} + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rosidl_runtime_c::rosidl_runtime_c +) +target_link_libraries(${PROJECT_NAME} PRIVATE + rcpputils::rcpputils ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -69,12 +68,7 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# specific order: dependents before dependencies -ament_export_dependencies(ament_cmake) -ament_export_dependencies(action_msgs) -ament_export_dependencies(rclcpp) -ament_export_dependencies(rcl_action) -ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(action_msgs rcl_action rclcpp rosidl_runtime_c) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -87,58 +81,52 @@ if(BUILD_TESTING) ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180) if(TARGET test_client) - ament_target_dependencies(test_client - "rcutils" - "test_msgs" - ) target_link_libraries(test_client ${PROJECT_NAME} mimick + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rcutils::rcutils + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180) if(TARGET test_server) - ament_target_dependencies(test_server - "rcpputils" - "rcutils" - "test_msgs" - ) target_link_libraries(test_server ${PROJECT_NAME} mimick + rcl_action::rcl_action + rclcpp::rclcpp + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp) if(TARGET test_server_goal_handle) - ament_target_dependencies(test_server_goal_handle - "rcutils" - "test_msgs" - ) target_link_libraries(test_server_goal_handle ${PROJECT_NAME} + ${action_msgs_TARGETS} mimick + rclcpp::rclcpp + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_traits test/test_traits.cpp) if(TARGET test_traits) - ament_target_dependencies(test_traits - "test_msgs" - ) target_link_libraries(test_traits ${PROJECT_NAME} + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_types test/test_types.cpp) if(TARGET test_types) - ament_target_dependencies(test_types - "test_msgs" - ) target_link_libraries(test_types ${PROJECT_NAME} + ${test_msgs_TARGETS} ) endif() endif() diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md index 7512ad3312..5812429610 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_action` packa The package `rclcpp_action` 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 Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +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). ## Version Policy [1] @@ -92,7 +92,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_action` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_action/tree/master/test) directory. +Each feature in `rclcpp_action` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_action/tree/rolling/test) directory. New features are required to have tests before being added. Currently nightly test results can be seen here: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `rclcpp_action` 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_action` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp_action/test/benchmark). +The performance tests of `rclcpp_action` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_action/test/benchmark). System level performance benchmarks that cover features of `rclcpp_action` can be found at: * [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_action` 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. +`rclcpp_action` 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. 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_action/) @@ -159,19 +159,19 @@ It also has several test dependencies, which do not affect the resulting quality `action_msgs` provides messages and services for ROS 2 actions. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/action_msgs/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). #### `rcl_action` The `rcl_action` package provides C-based ROS action implementation. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl_action/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md index c98987c922..dbb91a6ba3 100644 --- a/rclcpp_action/README.md +++ b/rclcpp_action/README.md @@ -2,7 +2,8 @@ Adds action APIs for C++. -Visit the [rclcpp_action API documentation](http://docs.ros2.org/latest/api/rclcpp_action/) for a complete list of its main components and features. For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). +The link to the latest rclcpp_action API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_action package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_action/). +For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). ## Quality Declaration diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a9bf2a5008..94cf4aab31 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -122,12 +122,12 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// \internal RCLCPP_ACTION_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t & wait_set) override; /// \internal RCLCPP_ACTION_PUBLIC @@ -142,7 +142,7 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// \internal /// Set a callback to be called when action client entities have an event diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index dd3a40671c..73987ec887 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -163,7 +163,7 @@ class ClientGoalHandle ResultCallback result_callback_{nullptr}; int8_t status_{GoalStatus::STATUS_ACCEPTED}; - std::mutex handle_mutex_; + std::recursive_mutex handle_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 0c25e52433..d12b4fc5b3 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -59,7 +59,7 @@ template std::shared_future::WrappedResult> ClientGoalHandle::async_get_result() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); if (!is_result_aware_) { throw exceptions::UnawareGoalHandleError(); } @@ -70,7 +70,7 @@ template void ClientGoalHandle::set_result(const WrappedResult & wrapped_result) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); status_ = static_cast(wrapped_result.code); result_promise_.set_value(wrapped_result); if (result_callback_) { @@ -82,7 +82,7 @@ template void ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); feedback_callback_ = callback; } @@ -90,7 +90,7 @@ template void ClientGoalHandle::set_result_callback(ResultCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); result_callback_ = callback; } @@ -98,7 +98,7 @@ template int8_t ClientGoalHandle::get_status() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return status_; } @@ -106,7 +106,7 @@ template void ClientGoalHandle::set_status(int8_t status) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); status_ = status; } @@ -114,7 +114,7 @@ template bool ClientGoalHandle::is_feedback_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return feedback_callback_ != nullptr; } @@ -122,7 +122,7 @@ template bool ClientGoalHandle::is_result_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return is_result_aware_; } @@ -130,7 +130,7 @@ template bool ClientGoalHandle::set_result_awareness(bool awareness) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); bool previous = is_result_aware_; is_result_aware_ = awareness; return previous; @@ -140,7 +140,7 @@ template void ClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); // Guard against multiple calls if (is_invalidated()) { return; @@ -168,7 +168,7 @@ ClientGoalHandle::call_feedback_callback( RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle."); return; } - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); if (nullptr == feedback_callback_) { // Normal, some feedback messages may arrive after the goal result. RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it."); diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 892de5743b..7461de2867 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -119,13 +119,13 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// Return true if any entity belonging to the action server is ready to be executed. /// \internal RCLCPP_ACTION_PUBLIC bool - is_ready(rcl_wait_set_t *) override; + is_ready(const rcl_wait_set_t & wait_set) override; RCLCPP_ACTION_PUBLIC std::shared_ptr @@ -139,7 +139,7 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// \internal /// Set a callback to be called when action server entities have an event @@ -279,19 +279,19 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute_goal_request_received(std::shared_ptr & data); + execute_goal_request_received(const std::shared_ptr & data); /// Handle a request to cancel goals on the server /// \internal RCLCPP_ACTION_PUBLIC void - execute_cancel_request_received(std::shared_ptr & data); + execute_cancel_request_received(const std::shared_ptr & data); /// Handle a request to get the result of an action /// \internal RCLCPP_ACTION_PUBLIC void - execute_result_request_received(std::shared_ptr & data); + execute_result_request_received(const std::shared_ptr & data); /// Handle a timeout indicating a completed goal should be forgotten by the server /// \internal diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index ac9dd49492..7d17819189 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -128,7 +128,7 @@ class Server; * accepted. * A `Server` will create an instance and give it to the user in their `handle_accepted` callback. * - * Internally, this class is responsible for coverting between the C++ action type and generic + * Internally, this class is responsible for converting between the C++ action type and generic * types for `rclcpp_action::ServerGoalHandleBase`. */ template @@ -196,7 +196,7 @@ class ServerGoalHandle : public ServerGoalHandleBase /// Indicate that a goal has been canceled. /** - * Only call this if the goal is executing or pending, but has been canceled. + * Only call this if the goal is canceling. * This is a terminal state, no more methods should be called on a goal handle after this is * called. * diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 829b6cd2c1..cf359dfaa9 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -69,14 +69,13 @@ struct hash { size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept { - // TODO(sloretz) Use someone else's hash function and cite it - size_t result = 0; - for (size_t i = 0; i < uuid.size(); ++i) { - for (size_t b = 0; b < sizeof(size_t); ++b) { - size_t part = uuid[i]; - part <<= CHAR_BIT * b; - result ^= part; - } + // Using the FNV-1a hash algorithm + constexpr size_t FNV_prime = 1099511628211u; + size_t result = 14695981039346656037u; + + for (const auto & byte : uuid) { + result ^= byte; + result *= FNV_prime; } return result; } diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index bfda7c1343..8c1dd30127 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 21.1.0 + 28.0.0 Adds action APIs for C++. Ivan Paunovic @@ -23,6 +23,7 @@ action_msgs rclcpp rcl_action + rcl rcpputils ament_cmake diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 2d5018d5af..fc5b3eeb90 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -163,7 +163,6 @@ bool ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) { auto start = std::chrono::steady_clock::now(); - // make an event to reuse, rather than create a new one each time auto node_ptr = pimpl_->node_graph_.lock(); if (!node_ptr) { throw rclcpp::exceptions::InvalidNodeError(); @@ -172,6 +171,7 @@ ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) if (this->action_server_is_ready()) { return true; } + // make an event to reuse, rather than create a new one each time auto event = node_ptr->get_graph_event(); if (timeout == std::chrono::nanoseconds(0)) { // check was non-blocking, return immediately @@ -253,20 +253,21 @@ ClientBase::get_number_of_ready_services() } void -ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) +ClientBase::add_to_wait_set(rcl_wait_set_t & wait_set) { rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, pimpl_->client_handle.get(), nullptr, nullptr); + &wait_set, pimpl_->client_handle.get(), nullptr, nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "ClientBase::add_to_wait_set() failed"); } } bool -ClientBase::is_ready(rcl_wait_set_t * wait_set) +ClientBase::is_ready(const rcl_wait_set_t & wait_set) { rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( - wait_set, pimpl_->client_handle.get(), + &wait_set, + pimpl_->client_handle.get(), &pimpl_->is_feedback_ready, &pimpl_->is_status_ready, &pimpl_->is_goal_response_ready, @@ -619,7 +620,7 @@ ClientBase::take_data_by_entity_id(size_t id) } void -ClientBase::execute(std::shared_ptr & data) +ClientBase::execute(const std::shared_ptr & data) { if (!data) { throw std::runtime_error("'data' is empty"); diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index c0fa96a88e..565aaa4f3b 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -165,18 +165,18 @@ ServerBase::get_number_of_ready_guard_conditions() } void -ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +ServerBase::add_to_wait_set(rcl_wait_set_t & wait_set) { std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_server( - wait_set, pimpl_->action_server_.get(), NULL); + &wait_set, pimpl_->action_server_.get(), NULL); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "ServerBase::add_to_wait_set() failed"); } } bool -ServerBase::is_ready(rcl_wait_set_t * wait_set) +ServerBase::is_ready(const rcl_wait_set_t & wait_set) { bool goal_request_ready; bool cancel_request_ready; @@ -186,7 +186,7 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) { std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); ret = rcl_action_server_wait_set_get_entities_ready( - wait_set, + &wait_set, pimpl_->action_server_.get(), &goal_request_ready, &cancel_request_ready, @@ -287,7 +287,7 @@ ServerBase::take_data_by_entity_id(size_t id) } void -ServerBase::execute(std::shared_ptr & data) +ServerBase::execute(const std::shared_ptr & data) { if (!data && !pimpl_->goal_expired_.load()) { throw std::runtime_error("'data' is empty"); @@ -307,7 +307,7 @@ ServerBase::execute(std::shared_ptr & data) } void -ServerBase::execute_goal_request_received(std::shared_ptr & data) +ServerBase::execute_goal_request_received(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast >>(data); @@ -344,7 +344,16 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) } if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send goal response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } else { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } const auto status = response_pair.first; @@ -396,11 +405,10 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) // Tell user to start executing action call_goal_accepted_callback(handle, uuid, message); } - data.reset(); } void -ServerBase::execute_cancel_request_received(std::shared_ptr & data) +ServerBase::execute_cancel_request_received(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast , @@ -483,14 +491,22 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) pimpl_->action_server_.get(), &request_header, response.get()); } + if (ret == RCL_RET_TIMEOUT) { + GoalUUID uuid = request->goal_info.goal_id.uuid; + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send cancel response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - data.reset(); } void -ServerBase::execute_result_request_received(std::shared_ptr & data) +ServerBase::execute_result_request_received(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast , rmw_request_id_t>>(data); @@ -538,11 +554,18 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); rcl_ret_t rcl_ret = rcl_action_send_result_response( pimpl_->action_server_.get(), &request_header, result_response.get()); + if (rcl_ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send result response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } if (RCL_RET_OK != rcl_ret) { rclcpp::exceptions::throw_from_rcl_error(rcl_ret); } } - data.reset(); } void @@ -671,7 +694,13 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_m for (auto & request_header : iter->second) { rcl_ret_t ret = rcl_action_send_result_response( pimpl_->action_server_.get(), &request_header, result_msg.get()); - if (RCL_RET_OK != ret) { + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send result response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } diff --git a/rclcpp_action/test/benchmark/CMakeLists.txt b/rclcpp_action/test/benchmark/CMakeLists.txt index dc87c553af..0ce6c6245c 100644 --- a/rclcpp_action/test/benchmark/CMakeLists.txt +++ b/rclcpp_action/test/benchmark/CMakeLists.txt @@ -9,8 +9,7 @@ add_performance_test( benchmark_action_client.cpp TIMEOUT 240) if(TARGET benchmark_action_client) - target_link_libraries(benchmark_action_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_action_client rclcpp test_msgs) + target_link_libraries(benchmark_action_client ${PROJECT_NAME} rclcpp::rclcpp ${test_msgs_TARGETS}) endif() add_performance_test( @@ -18,6 +17,5 @@ add_performance_test( benchmark_action_server.cpp TIMEOUT 120) if(TARGET benchmark_action_server) - target_link_libraries(benchmark_action_server ${PROJECT_NAME}) - ament_target_dependencies(benchmark_action_server rclcpp test_msgs) + target_link_libraries(benchmark_action_server ${PROJECT_NAME} rclcpp::rclcpp ${test_msgs_TARGETS}) endif() diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b94a82d500..08093cb873 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -397,13 +397,13 @@ TEST_F(TestClient, is_ready) { ASSERT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); - ASSERT_NO_THROW(action_client->add_to_wait_set(&wait_set)); - EXPECT_TRUE(action_client->is_ready(&wait_set)); + ASSERT_NO_THROW(action_client->add_to_wait_set(wait_set)); + EXPECT_TRUE(action_client->is_ready(wait_set)); { auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_client_wait_set_get_entities_ready, RCL_RET_ERROR); - EXPECT_THROW(action_client->is_ready(&wait_set), rclcpp::exceptions::RCLError); + EXPECT_THROW(action_client->is_ready(wait_set), rclcpp::exceptions::RCLError); } client_node.reset(); // Drop node before action client } @@ -499,8 +499,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait bool goal_response_received = false; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.goal_response_callback = - [&goal_response_received] - (typename ActionGoalHandle::SharedPtr goal_handle) + [&goal_response_received](typename ActionGoalHandle::SharedPtr goal_handle) { if (goal_handle) { goal_response_received = true; @@ -545,8 +544,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_ goal.order = 4; int feedback_count = 0; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); - send_goal_ops.feedback_callback = - [&feedback_count]( + send_goal_ops.feedback_callback = [&feedback_count]( typename ActionGoalHandle::SharedPtr goal_handle, const std::shared_ptr feedback) { @@ -852,6 +850,86 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); } +TEST_F(TestClientAgainstServer, deadlock_in_callbacks) +{ + std::atomic feedback_callback_called = false; + std::atomic response_callback_called = false; + std::atomic result_callback_called = false; + std::atomic no_deadlock = false; + + std::thread tr = std::thread( + [&]() { + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + + using GoalHandle = rclcpp_action::ClientGoalHandle; + rclcpp_action::Client::SendGoalOptions ops; + ops.feedback_callback = [&feedback_callback_called]( + const GoalHandle::SharedPtr handle, ActionType::Feedback::ConstSharedPtr) + { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + feedback_callback_called = true; + }; + ops.goal_response_callback = [&response_callback_called]( + const GoalHandle::SharedPtr & handle) { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + response_callback_called = true; + }; + ops.result_callback = [&result_callback_called]( + const GoalHandle::WrappedResult &) { + result_callback_called = true; + }; + + goal.order = 6; + auto future_goal_handle = action_client->async_send_goal(goal, ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + ASSERT_TRUE(goal_handle); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + auto result_future = action_client->async_get_result(goal_handle); + dual_spin_until_future_complete(result_future); + + EXPECT_TRUE(result_future.valid()); + auto result = result_future.get(); + + no_deadlock = true; + }); + + auto start_time = std::chrono::system_clock::now(); + + while (std::chrono::system_clock::now() - start_time < std::chrono::milliseconds(2000) && + !no_deadlock) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (no_deadlock) { + tr.join(); + } else { + // In case of a failure, the thread is assumed to be in a deadlock. + // We detach the thread so we don't block further tests. + tr.detach(); + } + + EXPECT_TRUE(no_deadlock); + EXPECT_TRUE(response_callback_called); + EXPECT_TRUE(result_callback_called); + EXPECT_TRUE(feedback_callback_called); +} + TEST_F(TestClientAgainstServer, send_rcl_errors) { auto action_client = rclcpp_action::create_client(client_node, action_name); @@ -860,9 +938,8 @@ TEST_F(TestClientAgainstServer, send_rcl_errors) auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.result_callback = [](const typename ActionGoalHandle::WrappedResult &) {}; - send_goal_ops.feedback_callback = - [](typename ActionGoalHandle::SharedPtr, - const std::shared_ptr) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; { ActionGoal goal; @@ -902,9 +979,8 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.result_callback = [](const typename ActionGoalHandle::WrappedResult &) {}; - send_goal_ops.feedback_callback = - [](typename ActionGoalHandle::SharedPtr, - const std::shared_ptr) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; { ActionGoal goal; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 8687f90fbe..9ffa31797f 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1107,12 +1107,12 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); - EXPECT_NO_THROW(action_server_->add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(action_server_->add_to_wait_set(wait_set)); - EXPECT_TRUE(action_server_->is_ready(&wait_set)); + EXPECT_TRUE(action_server_->is_ready(wait_set)); auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_server_wait_set_get_entities_ready, RCL_RET_ERROR); - EXPECT_THROW(action_server_->is_ready(&wait_set), rclcpp::exceptions::RCLError); + EXPECT_THROW(action_server_->is_ready(wait_set), rclcpp::exceptions::RCLError); } TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal_request_errors) diff --git a/rclcpp_action/test/test_types.cpp b/rclcpp_action/test/test_types.cpp index 4922c52930..619a4f7899 100644 --- a/rclcpp_action/test/test_types.cpp +++ b/rclcpp_action/test/test_types.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "rclcpp_action/types.hpp" TEST(TestActionTypes, goal_uuid_to_string) { @@ -59,3 +60,35 @@ TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); } } + +TEST(TestActionTypes, goal_uuid_to_hashed_uuid_random) { + // Use std::random_device to seed the generator of goal IDs. + std::random_device rd; + std::independent_bits_engine< + std::default_random_engine, 8, decltype(rd())> random_bytes_generator(rd()); + + std::vector hashed_guuids; + constexpr size_t iterations = 1000; + + for (size_t i = 0; i < iterations; i++) { + rclcpp_action::GoalUUID goal_id; + + // Generate random bytes for each element of the array + for (auto & element : goal_id) { + element = static_cast(random_bytes_generator()); + } + + size_t new_hashed_guuid = std::hash()(goal_id); + + // Search for any prevoius hashed goal_id with the same value + for (auto prev_hashed_guuid : hashed_guuids) { + EXPECT_NE(prev_hashed_guuid, new_hashed_guuid); + if (prev_hashed_guuid == new_hashed_guuid) { + // Fail before the first occurrence of a collision + GTEST_FAIL(); + } + } + + hashed_guuids.push_back(new_hashed_guuid); + } +} diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 144faaf2a5..ad7db5096e 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,63 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.0 (2024-03-28) +------------------- +* Add EXECUTOR docs (`#2440 `_) +* Update quality declaration documents (`#2427 `_) +* crash on no class found (`#2415 `_) + * crash on no class found + * error on no class found instead of no callback groups + Co-authored-by: Chris Lalancette +* Contributors: Adam Aposhian, Christophe Bedard, Ruddick Lawrence + +27.0.0 (2024-02-07) +------------------- + +26.0.0 (2024-01-24) +------------------- + +25.0.0 (2023-12-26) +------------------- +* Switch to target_link_libraries. (`#2374 `_) +* feat(rclcpp_components): support events executor in node main template (`#2366 `_) +* fix(rclcpp_components): increase the service queue sizes in component_container (`#2363 `_) +* Contributors: Chris Lalancette, Daisuke Nishimatsu, M. Fatih Cırıt + +24.0.0 (2023-11-06) +------------------- + +23.2.0 (2023-10-09) +------------------- + +23.1.0 (2023-10-04) +------------------- +* Add missing header required by the rclcpp::NodeOptions type (`#2324 `_) +* Contributors: Ignacio Vizzo + +23.0.0 (2023-09-08) +------------------- +* Update API docs links in package READMEs (`#2302 `_) +* Contributors: Christophe Bedard + +22.2.0 (2023-09-07) +------------------- + +22.1.0 (2023-08-21) +------------------- + +22.0.0 (2023-07-11) +------------------- + +21.3.0 (2023-06-12) +------------------- + +21.2.0 (2023-06-07) +------------------- + +21.1.1 (2023-05-11) +------------------- + 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 121ef434c2..7d4135051c 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(composition_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) -# Add an interface library that can be dependend upon by libraries who register components +# Add an interface library that can be depended upon by libraries who register components add_library(component INTERFACE) target_include_directories(component INTERFACE "$" @@ -38,12 +38,14 @@ add_library( target_include_directories(component_manager PUBLIC "$" "$") -ament_target_dependencies(component_manager - "ament_index_cpp" - "class_loader" - "composition_interfaces" - "rclcpp" - "rcpputils" +target_link_libraries(component_manager PUBLIC + ${composition_interfaces_TARGETS} + rclcpp::rclcpp +) +target_link_libraries(component_manager PRIVATE + ament_index_cpp::ament_index_cpp + class_loader::class_loader + rcpputils::rcpputils ) target_compile_definitions(component_manager PRIVATE "RCLCPP_COMPONENTS_BUILDING_LIBRARY") @@ -52,10 +54,7 @@ add_executable( component_container src/component_container.cpp ) -target_link_libraries(component_container component_manager) -ament_target_dependencies(component_container - "rclcpp" -) +target_link_libraries(component_container component_manager rclcpp::rclcpp) set(node_main_template_install_dir "share/${PROJECT_NAME}") install(FILES @@ -66,20 +65,13 @@ add_executable( component_container_mt src/component_container_mt.cpp ) -target_link_libraries(component_container_mt component_manager) -ament_target_dependencies(component_container_mt - "rclcpp" -) +target_link_libraries(component_container_mt component_manager rclcpp::rclcpp) add_executable( component_container_isolated src/component_container_isolated.cpp ) -target_link_libraries(component_container_isolated component_manager) -ament_target_dependencies(component_container_isolated - "rclcpp" -) - +target_link_libraries(component_container_isolated component_manager rclcpp::rclcpp) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") target_link_libraries(component_container "stdc++fs") diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md index 5b8efa934d..2e38c0410a 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_components` p The package `rclcpp_components` claims to be in the **Quality Level 1** category. -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. +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). ## Version Policy [1] @@ -92,7 +92,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_components` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_components/tree/master/test) directory. +Each feature in `rclcpp_components` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_components/tree/rolling/test) directory. New features are required to have tests before being added. Currently nightly test results can be seen here: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `rclcpp_components` 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_components` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp_components/test/benchmark). +The performance tests of `rclcpp_components` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components/test/benchmark). Package and system level performance benchmarks that cover features of `rclcpp_components` can be found at: * [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_components` 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. +`rclcpp_components` 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. 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_components/) @@ -159,7 +159,7 @@ It also has several test dependencies, which do not affect the resulting quality The `ament_index_cpp` package provides a C++ API to access the ament resource index. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/master/ament_index_cpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/rolling/ament_index_cpp/QUALITY_DECLARATION.md). #### `class_loader` @@ -171,19 +171,19 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://github The `composition_interfaces` package contains message and service definitions for managing composable nodes in a container process. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/composition_interfaces/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/composition_interfaces/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/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/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md index 829bcbc888..0b0e0a7aec 100644 --- a/rclcpp_components/README.md +++ b/rclcpp_components/README.md @@ -2,7 +2,7 @@ Package containing tools for dynamically loadable components. -Visit the [rclcpp_components API documentation](http://docs.ros2.org/latest/api/rclcpp_components/) for a complete list of its main components and features. +The link to the latest rclcpp_components API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_components package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_components/). ## Quality Declaration diff --git a/rclcpp_components/cmake/rclcpp_components_register_node.cmake b/rclcpp_components/cmake/rclcpp_components_register_node.cmake index c5b87af667..0c5bd33b91 100644 --- a/rclcpp_components/cmake/rclcpp_components_register_node.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -24,6 +24,8 @@ # :type PLUGIN: string # :param EXECUTABLE: the node's executable name # :type EXECUTABLE: string +# :param EXECUTOR: the C++ class name of the executor to use (blank uses SingleThreadedExecutor) +# :type EXECUTOR: string # :param RESOURCE_INDEX: the ament resource index to register the components # :type RESOURCE_INDEX: string # @@ -70,10 +72,11 @@ macro(rclcpp_components_register_node target) file(GENERATE OUTPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp INPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_configured_${node}.cpp.in) add_executable(${node} ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp) - ament_target_dependencies(${node} - "rclcpp" - "class_loader" - "rclcpp_components") + target_link_libraries(${node} + class_loader::class_loader + rclcpp::rclcpp + rclcpp_components::component + ) install(TARGETS ${node} DESTINATION lib/${PROJECT_NAME}) diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp index 67e6cd7331..7b1f2dcae6 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ #define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ +#include "rclcpp/node_options.hpp" #include "rclcpp_components/node_instance_wrapper.hpp" namespace rclcpp_components diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 5a63923544..e309698b84 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 21.1.0 + 28.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 036350a64f..7b77af9c92 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -39,10 +39,12 @@ ComponentManager::ComponentManager( { loadNode_srv_ = create_service( "~/_container/load_node", - std::bind(&ComponentManager::on_load_node, this, _1, _2, _3)); + std::bind(&ComponentManager::on_load_node, this, _1, _2, _3), + rclcpp::ServicesQoS().keep_last(200)); unloadNode_srv_ = create_service( "~/_container/unload_node", - std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3)); + std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3), + rclcpp::ServicesQoS().keep_last(200)); listNodes_srv_ = create_service( "~/_container/list_nodes", std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3)); diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index 71754d1f82..7d621aac9f 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -23,48 +24,56 @@ #define NODE_MAIN_LOGGER_NAME "@node@" +using namespace rclcpp::executors; +using namespace rclcpp::experimental::executors; + int main(int argc, char * argv[]) { auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); rclcpp::Logger logger = rclcpp::get_logger(NODE_MAIN_LOGGER_NAME); - rclcpp::executors::@executor@ exec; + @executor@ exec; rclcpp::NodeOptions options; options.arguments(args); - std::vector node_wrappers; std::string library_name = "@library_name@"; std::string class_name = "rclcpp_components::NodeFactoryTemplate<@component@>"; RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str()); auto loader = std::make_unique(library_name); - auto classes = loader->getAvailableClasses(); - for (const auto & clazz : classes) { - std::string name = clazz.c_str(); - if (name.compare(class_name) == 0) { - RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str()); - std::shared_ptr node_factory = nullptr; - try { - node_factory = loader->createInstance(clazz); - } catch (const std::exception & ex) { - RCLCPP_ERROR(logger, "Failed to load library %s", ex.what()); - return 1; - } catch (...) { - RCLCPP_ERROR(logger, "Failed to load library"); - return 1; - } - auto wrapper = node_factory->create_node_instance(options); - auto node = wrapper.get_node_base_interface(); - node_wrappers.push_back(wrapper); - exec.add_node(node); - } + std::vector classes = loader->getAvailableClasses(); + + if (std::find( + classes.begin(), + classes.end(), + class_name) == classes.end()) { + RCLCPP_ERROR( + logger, + "Class %s not found in library %s", + class_name.c_str(), + library_name.c_str()); + return 1; + } + RCLCPP_DEBUG(logger, "Instantiate class %s", class_name.c_str()); + std::shared_ptr node_factory = nullptr; + try { + node_factory = loader->createInstance(class_name); + } catch (const std::exception & ex) { + RCLCPP_ERROR(logger, "Failed to load library %s", ex.what()); + return 1; + } catch (...) { + RCLCPP_ERROR(logger, "Failed to load library"); + return 1; } + // Scope to destruct node_wrapper before shutdown + { + rclcpp_components::NodeInstanceWrapper node_wrapper = node_factory->create_node_instance(options); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node = node_wrapper.get_node_base_interface(); + exec.add_node(node); - exec.spin(); + exec.spin(); - for (auto wrapper : node_wrappers) { - exec.remove_node(wrapper.get_node_base_interface()); + exec.remove_node(node_wrapper.get_node_base_interface()); } - node_wrappers.clear(); rclcpp::shutdown(); diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 9ab0142005..0e8c604097 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,66 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.0 (2024-03-28) +------------------- +* Update quality declaration documents (`#2427 `_) +* Contributors: Christophe Bedard + +27.0.0 (2024-02-07) +------------------- + +26.0.0 (2024-01-24) +------------------- +* Increase timeout for rclcpp_lifecycle to 360 (`#2395 `_) +* Contributors: Jorge Perez + +25.0.0 (2023-12-26) +------------------- + +24.0.0 (2023-11-06) +------------------- +* Fix rclcpp_lifecycle inclusion on Windows. (`#2331 `_) +* Contributors: Chris Lalancette + +23.2.0 (2023-10-09) +------------------- +* add clients & services count (`#2072 `_) +* Contributors: Minju, Lee + +23.1.0 (2023-10-04) +------------------- + +23.0.0 (2023-09-08) +------------------- +* Update API docs links in package READMEs (`#2302 `_) +* Contributors: Christophe Bedard + +22.2.0 (2023-09-07) +------------------- +* add logger level service to lifecycle node. (`#2277 `_) +* Contributors: Tomoya Fujita + +22.1.0 (2023-08-21) +------------------- +* Stop using constref signature of benchmark DoNotOptimize. (`#2238 `_) +* Contributors: Chris Lalancette + +22.0.0 (2023-07-11) +------------------- +* Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) +* Switch lifecycle to use the RCLCPP macros. (`#2233 `_) +* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) +* Contributors: Chris Lalancette, Emerson Knapp + +21.3.0 (2023-06-12) +------------------- + +21.2.0 (2023-06-07) +------------------- + +21.1.1 (2023-05-11) +------------------- + 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index f46f993b21..66b1990ec5 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -60,8 +60,13 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # Give cppcheck hints about macro definitions coming from outside this package set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS}) + list(APPEND AMENT_LINT_AUTO_EXCLUDE "ament_cmake_cppcheck") ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck() + set_tests_properties(cppcheck PROPERTIES TIMEOUT 360) + find_package(performance_test_fixture REQUIRED) add_performance_test( diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md index 5a37ad395b..73be7c6c9a 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_lifecycle` pa The package `rclcpp_lifecycle` claims to be in the **Quality Level 1** category when 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 Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +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). ## Version Policy [1] @@ -92,7 +92,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_lifecycle` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_lifecycle/tree/master/test) directory. +Each feature in `rclcpp_lifecycle` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_lifecycle/tree/rolling/test) directory. New features are required to have tests before being added. Currently nightly test results can be seen here: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `rclcpp_lifecycle` 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_lifecycle` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp_lifecycle/test/benchmark). +The performance tests of `rclcpp_lifecycle` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_lifecycle/test/benchmark). Package and system level performance benchmarks that cover features of `rclcpp_lifecycle` can be found at: * [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_lifecycle` 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. +`rclcpp_lifecycle` 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. 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_lifecycle/) @@ -159,31 +159,31 @@ It also has several test dependencies, which do not affect the resulting quality The `lifecycle_msgs` package contains message and service definitions for managing lifecycle nodes. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/lifecycle_msgs/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). #### `rcl_lifecycle` The `rcl_lifecycle` package provides functionality for ROS 2 lifecycle nodes in C. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_lifecycle/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl_lifecycle/QUALITY_DECLARATION.md). #### `rosidl_typesupport_cpp` The `rosidl_typesupport_cpp` package generates the type support for C++ messages. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/rolling/rosidl_typesupport_cpp/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/master/rmw/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/rmw/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md index d5bcd96479..e182390136 100644 --- a/rclcpp_lifecycle/README.md +++ b/rclcpp_lifecycle/README.md @@ -2,7 +2,8 @@ Package containing a prototype for lifecycle implementation. -Visit the [rclcpp_lifecycle API documentation](http://docs.ros2.org/latest/api/rclcpp_lifecycle/) for a complete list of its main components and features. For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). +The link to the latest rclcpp_lifecycle API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_lifecycle package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_lifecycle/). +For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). ## Quality Declaration diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 197ecf5c19..657ddddc34 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -72,6 +72,7 @@ #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/parameter.hpp" #include "rclcpp/publisher.hpp" @@ -689,6 +690,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, size_t count_subscribers(const std::string & topic_name) const; + /// Return the number of clients created for a given service. + /** + * \sa rclcpp::Node::count_clients + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_clients(const std::string & service_name) const; + + /// Return the number of services created for a given service. + /** + * \sa rclcpp::Node::count_services + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_services(const std::string & service_name) const; + /// Return the topic endpoint information about publishers on a given topic. /** * \sa rclcpp::Node::get_publishers_info_by_topic @@ -823,6 +840,14 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); + /// Return the Node's internal NodeTypeDescriptionsInterface implementation. + /** + * \sa rclcpp::Node::get_node_type_descriptions_interface + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr + get_node_type_descriptions_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. /** * \sa rclcpp::Node::get_node_waitables_interface @@ -1085,6 +1110,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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_; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 272c4def27..45748ea55d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -21,6 +21,14 @@ #include "rclcpp_lifecycle/visibility_control.h" #include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +// When windows.h is included, ERROR is defined as a macro. So the use of it later in this file, +// even as an enum, causes compilation errors. Work around this by undefining the macro here, +// and then redefining when this header is finished being included. +#if defined(_WIN32) +#pragma push_macro("ERROR") +#undef ERROR +#endif + namespace rclcpp_lifecycle { namespace node_interfaces @@ -108,6 +116,10 @@ class LifecycleNodeInterface } // namespace node_interfaces } // namespace rclcpp_lifecycle +#if defined(_WIN32) +#pragma pop_macro("ERROR") +#endif + RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, lifecycle_node) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index ac710d1431..6a2157c002 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 21.1.0 + 28.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4c0b94cb42..0c448cf5e6 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -43,6 +43,7 @@ #include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/parameter_service.hpp" #include "rclcpp/qos.hpp" @@ -113,9 +114,15 @@ LifecycleNode::LifecycleNode( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, + node_services_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), - impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_)) { impl_->init(enable_communication_interface); @@ -137,6 +144,10 @@ LifecycleNode::LifecycleNode( &LifecycleNodeInterface::on_deactivate, this, std::placeholders::_1)); register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1)); + + if (options.enable_logger_service()) { + node_logging_->create_logger_services(node_services_); + } } LifecycleNode::~LifecycleNode() @@ -375,6 +386,18 @@ LifecycleNode::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +size_t +LifecycleNode::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +LifecycleNode::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + std::vector LifecycleNode::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const { @@ -468,6 +491,12 @@ LifecycleNode::get_node_topics_interface() return node_topics_; } +rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr +LifecycleNode::get_node_type_descriptions_interface() +{ + return node_type_descriptions_; +} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr LifecycleNode::get_node_services_interface() { diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 5c5f7797e1..9074c9cc6e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -29,6 +29,7 @@ #include "lifecycle_msgs/srv/get_available_transitions.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -50,9 +51,11 @@ namespace rclcpp_lifecycle LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface) + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface) : node_base_interface_(node_base_interface), - node_services_interface_(node_services_interface) + node_services_interface_(node_services_interface), + node_logging_interface_(node_logging_interface) { } @@ -65,8 +68,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); } if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", + RCLCPP_FATAL( + node_logging_interface_->get_logger(), "failed to destroy rcl_state_machine"); } } @@ -398,7 +401,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( { std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Unable to change state for state machine for %s: %s", node_base_interface_->get_name(), rcl_get_error_string().str); return RCL_RET_ERROR; @@ -411,7 +415,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_id( &state_machine_, transition_id, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Unable to start transition %u from current state %s: %s", transition_id, state_machine_.current_state->label, rcl_get_error_string().str); rcutils_reset_error(); @@ -443,7 +448,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_label( &state_machine_, transition_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Failed to finish transition %u. Current state is now: %s (%s)", transition_id, state_machine_.current_state->label, rcl_get_error_string().str); rcutils_reset_error(); @@ -458,7 +464,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling."); + RCLCPP_WARN( + node_logging_interface_->get_logger(), + "Error occurred while doing error handling."); auto error_cb_code = execute_callback(current_state_id, initial_state); auto error_cb_label = get_label_for_return_code(error_cb_code); @@ -467,7 +475,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_label( &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Failed to call cleanup on error state: %s", rcl_get_error_string().str); rcutils_reset_error(); return RCL_RET_ERROR; } @@ -495,8 +505,12 @@ LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( try { cb_success = callback(State(previous_state)); } catch (const std::exception & e) { - RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); - RCUTILS_LOG_ERROR("Original error: %s", e.what()); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Caught exception in callback for transition %d", it->first); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Original error: %s", e.what()); cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d09f44831c..5cf5bdaacf 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -32,6 +32,7 @@ #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_services_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -52,7 +53,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface); + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface); ~LifecycleNodeInterfaceImpl(); @@ -152,6 +154,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; + using NodeLoggingPtr = std::shared_ptr; using ChangeStateSrvPtr = std::shared_ptr>; using GetStateSrvPtr = std::shared_ptr>; using GetAvailableStatesSrvPtr = @@ -163,6 +166,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final NodeBasePtr node_base_interface_; NodeServicesPtr node_services_interface_; + NodeLoggingPtr node_logging_interface_; ChangeStateSrvPtr srv_change_state_; GetStateSrvPtr srv_get_state_; GetAvailableStatesSrvPtr srv_get_available_states_; diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp index c8d166ef66..59ed24489e 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -228,7 +228,8 @@ class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTes BENCHMARK_F(BenchmarkLifecycleClient, get_state)(benchmark::State & state) { for (auto _ : state) { (void)_; - const auto lifecycle_state = lifecycle_client->get_state(); + + lifecycle_msgs::msg::State lifecycle_state = lifecycle_client->get_state(); if (lifecycle_state.id != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { const std::string msg = std::string("Expected state did not match actual: ") + @@ -268,7 +269,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & s for (auto _ : state) { (void)_; constexpr size_t expected_states = 11u; - const auto states = lifecycle_client->get_available_states(); + std::vector states = lifecycle_client->get_available_states(); if (states.size() != expected_states) { const std::string msg = std::string("Expected number of states did not match actual: ") + @@ -284,7 +285,8 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::Stat for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 2u; - const auto transitions = lifecycle_client->get_available_transitions(); + std::vector transitions = + lifecycle_client->get_available_transitions(); if (transitions.size() != expected_transitions) { const std::string msg = std::string("Expected number of transitions did not match actual: ") + @@ -300,7 +302,8 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_transition_graph)(benchmark::State & s for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 25u; - const auto transitions = lifecycle_client->get_transition_graph(); + std::vector transitions = + lifecycle_client->get_transition_graph(); if (transitions.size() != expected_transitions) { const std::string msg = std::string("Expected number of transitions did not match actual: ") + diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp index 0bf3c48ab9..4be9ad13af 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp @@ -97,13 +97,15 @@ class BenchmarkLifecycleNode : public performance_test_fixture::PerformanceTest BENCHMARK_F(BenchmarkLifecycleNode, get_current_state)(benchmark::State & state) { for (auto _ : state) { (void)_; - const auto & lifecycle_state = node->get_current_state(); + const rclcpp_lifecycle::State & lifecycle_state = node->get_current_state(); if (lifecycle_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { const std::string message = std::string("Node's current state is: ") + std::to_string(lifecycle_state.id()); state.SkipWithError(message.c_str()); } - benchmark::DoNotOptimize(lifecycle_state); + // Google benchmark 1.8.2 warns that the constref DoNotOptimize signature may be optimized away + // by the compiler. Cast const away to ensure we don't get that problem (and warning). + benchmark::DoNotOptimize(const_cast(lifecycle_state)); benchmark::ClobberMemory(); } } @@ -112,7 +114,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & sta for (auto _ : state) { (void)_; constexpr size_t expected_states = 11u; - const auto lifecycle_states = node->get_available_states(); + std::vector lifecycle_states = node->get_available_states(); if (lifecycle_states.size() != expected_states) { const std::string msg = std::to_string(lifecycle_states.size()); state.SkipWithError(msg.c_str()); @@ -126,7 +128,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 2u; - const auto & transitions = node->get_available_transitions(); + std::vector transitions = node->get_available_transitions(); if (transitions.size() != expected_transitions) { const std::string msg = std::to_string(transitions.size()); state.SkipWithError(msg.c_str()); @@ -140,7 +142,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_transition_graph)(benchmark::State & sta for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 25u; - const auto & transitions = node->get_transition_graph(); + std::vector transitions = node->get_transition_graph(); if (transitions.size() != expected_transitions) { const std::string msg = std::string("Expected number of transitions did not match actual: ") + @@ -162,18 +164,20 @@ BENCHMARK_F(BenchmarkLifecycleNode, transition_valid_state)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { (void)_; - const auto & active = + const rclcpp_lifecycle::State & active = node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); if (active.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { state.SkipWithError("Transition to active state failed"); } - const auto & inactive = + const rclcpp_lifecycle::State & inactive = node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); if (inactive.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { state.SkipWithError("Transition to inactive state failed"); } - benchmark::DoNotOptimize(active); - benchmark::DoNotOptimize(inactive); + // Google benchmark 1.8.2 warns that the constref DoNotOptimize signature may be optimized away + // by the compiler. Cast const away to ensure we don't get that problem (and warning). + benchmark::DoNotOptimize(const_cast(active)); + benchmark::DoNotOptimize(const_cast(inactive)); benchmark::ClobberMemory(); } } diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 31850c589b..fdb9be6153 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -25,6 +25,8 @@ #include "lifecycle_msgs/msg/transition.hpp" #include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -34,6 +36,8 @@ using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; +using namespace std::chrono_literals; + static const std::chrono::nanoseconds DEFAULT_EVENT_TIMEOUT = std::chrono::seconds(3); static const std::chrono::nanoseconds DEFAULT_EVENT_SLEEP_PERIOD = std::chrono::milliseconds(100); @@ -249,6 +253,35 @@ TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) { } } +TEST_F(TestDefaultStateMachine, check_logger_services_exist) { + // Logger level services are disabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(false); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_FALSE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_FALSE(set_client->wait_for_service(2s)); + } + // Logger level services are enabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(true); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(set_client->wait_for_service(2s)); + } +} + TEST_F(TestDefaultStateMachine, trigger_transition) { auto test_node = std::make_shared("testnode"); @@ -427,11 +460,15 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { // Parameters are tested more thoroughly in rclcpp's test_node.cpp // These are provided for coverage of lifecycle node's API TEST_F(TestDefaultStateMachine, declare_parameters) { + // "start_type_description_service" and "use_sim_time" + const uint64_t builtin_param_count = 2; + const uint64_t expected_param_count = 6 + builtin_param_count; auto test_node = std::make_shared("testnode"); auto list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); const std::string bool_name = "test_boolean"; const std::string int_name = "test_int"; @@ -469,10 +506,11 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { test_node->declare_parameters("test_double", double_parameters); list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 7u); + EXPECT_EQ(list_result.names.size(), expected_param_count); // The order of these names is not controlled by lifecycle_node, doing set equality std::set expected_names = { + "start_type_description_service", "test_boolean", "test_double.double_one", "test_double.double_two", @@ -487,11 +525,13 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { } TEST_F(TestDefaultStateMachine, check_parameters) { + const uint64_t builtin_param_count = 2; auto test_node = std::make_shared("testnode"); auto list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); const std::string bool_name = "test_boolean"; const std::string int_name = "test_int"; @@ -549,8 +589,7 @@ TEST_F(TestDefaultStateMachine, check_parameters) { std::map parameter_map; EXPECT_TRUE(test_node->get_parameters({}, parameter_map)); - // int param, bool param, and use_sim_time - EXPECT_EQ(parameter_map.size(), 3u); + EXPECT_EQ(parameter_map.size(), parameter_names.size() + builtin_param_count); // Check parameter types auto parameter_types = test_node->get_parameter_types(parameter_names); @@ -585,10 +624,12 @@ TEST_F(TestDefaultStateMachine, check_parameters) { // List parameters list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 3u); - EXPECT_STREQ(list_result.names[0].c_str(), parameter_names[0].c_str()); - EXPECT_STREQ(list_result.names[1].c_str(), parameter_names[1].c_str()); - EXPECT_STREQ(list_result.names[2].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), parameter_names.size() + builtin_param_count); + size_t index = 0; + EXPECT_STREQ(list_result.names[index++].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[0].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[1].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), "use_sim_time"); // Undeclare parameter test_node->undeclare_parameter(bool_name); @@ -633,6 +674,7 @@ TEST_F(TestDefaultStateMachine, test_getters) { EXPECT_LT(0u, test_node->now().nanoseconds()); EXPECT_STREQ("testnode", test_node->get_logger().get_name()); EXPECT_NE(nullptr, const_cast(test_node.get())->get_clock()); + EXPECT_NE(nullptr, test_node->get_node_type_descriptions_interface()); } TEST_F(TestDefaultStateMachine, test_graph_topics) { @@ -684,6 +726,17 @@ TEST_F(TestDefaultStateMachine, test_graph_services) { EXPECT_STREQ( service_names_and_types["/testnode/get_transition_graph"][0].c_str(), "lifecycle_msgs/srv/GetAvailableTransitions"); + + EXPECT_EQ(0u, test_node->count_clients("/testnode/change_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_states")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_transitions")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_transition_graph")); + EXPECT_EQ(1u, test_node->count_services("/testnode/change_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_states")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_transitions")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_transition_graph")); } TEST_F(TestDefaultStateMachine, test_graph_services_by_node) {