Skip to content

Commit aaf8b38

Browse files
wjwwoodwjwwood
authored
[rclcpp] add WaitSet class and modify entities to work without executor (ros2#1047)
* add rclcpp::GuardCondition wrapping rcl_guard_condition_t Signed-off-by: William Woodall <william@osrfoundation.org> * WIP second wait set refactor, just guard conditions so far Signed-off-by: William Woodall <william@osrfoundation.org> * fix typo Signed-off-by: William Woodall <william@osrfoundation.org> * removing a question/todo, I think this is fine as is Signed-off-by: William Woodall <william@osrfoundation.org> * added subscriptions and waitable to wait sets Signed-off-by: William Woodall <william@osrfoundation.org> * improve usability with subscriptions and wait sets Signed-off-by: William Woodall <william@osrfoundation.org> * adding take to subscription so it can be used without the executor Signed-off-by: William Woodall <william@osrfoundation.org> * add rclcpp::MessageInfo to replace use of rmw_message_info_t Signed-off-by: William Woodall <william@osrfoundation.org> * refactor Subscription and Executor so they can be used separately Signed-off-by: William Woodall <william@osrfoundation.org> * style and cpplint Signed-off-by: William Woodall <william@osrfoundation.org> * fixup take_serialized() and add tests for it Signed-off-by: William Woodall <william@osrfoundation.org> * add support for client and service to wait set Signed-off-by: William Woodall <william@osrfoundation.org> * fix typo Signed-off-by: William Woodall <william@osrfoundation.org> * fix typo Signed-off-by: William Woodall <william@osrfoundation.org> * fix review comment Signed-off-by: William Woodall <william@osrfoundation.org> * add thread-safe wait set policy Signed-off-by: William Woodall <william@osrfoundation.org> * add check for use with multiple wait set Signed-off-by: William Woodall <william@osrfoundation.org> * fixup visibility macro usage Signed-off-by: William Woodall <william@osrfoundation.org> * remove vestigial test case Signed-off-by: William Woodall <william@osrfoundation.org> * move visibility macro fixes Signed-off-by: William Woodall <william@osrfoundation.org> * remove vestigial TODO Signed-off-by: William Woodall <william@osrfoundation.org>
1 parent 0f0b833 commit aaf8b38

37 files changed

Lines changed: 4511 additions & 139 deletions

‎rclcpp/CMakeLists.txt‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,11 +49,13 @@ set(${PROJECT_NAME}_SRCS
4949
src/rclcpp/executors/static_executor_entities_collector.cpp
5050
src/rclcpp/executors/static_single_threaded_executor.cpp
5151
src/rclcpp/graph_listener.cpp
52+
src/rclcpp/guard_condition.cpp
5253
src/rclcpp/init_options.cpp
5354
src/rclcpp/intra_process_manager.cpp
5455
src/rclcpp/logger.cpp
5556
src/rclcpp/memory_strategies.cpp
5657
src/rclcpp/memory_strategy.cpp
58+
src/rclcpp/message_info.cpp
5759
src/rclcpp/node.cpp
5860
src/rclcpp/node_options.cpp
5961
src/rclcpp/node_interfaces/node_base.cpp
@@ -84,6 +86,7 @@ set(${PROJECT_NAME}_SRCS
8486
src/rclcpp/timer.cpp
8587
src/rclcpp/type_support.cpp
8688
src/rclcpp/utilities.cpp
89+
src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp
8790
src/rclcpp/waitable.cpp
8891
)
8992

@@ -519,6 +522,19 @@ if(BUILD_TESTING)
519522
target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
520523
endif()
521524

525+
ament_add_gtest(test_guard_condition test/test_guard_condition.cpp
526+
APPEND_LIBRARY_DIRS "${append_library_dirs}")
527+
if(TARGET test_guard_condition)
528+
target_link_libraries(test_guard_condition ${PROJECT_NAME})
529+
endif()
530+
531+
ament_add_gtest(test_wait_set test/test_wait_set.cpp
532+
APPEND_LIBRARY_DIRS "${append_library_dirs}")
533+
if(TARGET test_wait_set)
534+
ament_target_dependencies(test_wait_set "test_msgs")
535+
target_link_libraries(test_wait_set ${PROJECT_NAME})
536+
endif()
537+
522538
# Install test resources
523539
install(
524540
DIRECTORY test/resources

‎rclcpp/include/rclcpp/any_subscription_callback.hpp‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525

2626
#include "rclcpp/allocator/allocator_common.hpp"
2727
#include "rclcpp/function_traits.hpp"
28+
#include "rclcpp/message_info.hpp"
2829
#include "rclcpp/visibility_control.hpp"
2930
#include "tracetools/tracetools.h"
3031
#include "tracetools/utils.hpp"
@@ -43,13 +44,13 @@ class AnySubscriptionCallback
4344

4445
using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
4546
using SharedPtrWithInfoCallback =
46-
std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
47+
std::function<void (const std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>;
4748
using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
4849
using ConstSharedPtrWithInfoCallback =
49-
std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
50+
std::function<void (const std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>;
5051
using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
5152
using UniquePtrWithInfoCallback =
52-
std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
53+
std::function<void (MessageUniquePtr, const rclcpp::MessageInfo &)>;
5354

5455
SharedPtrCallback shared_ptr_callback_;
5556
SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -155,7 +156,7 @@ class AnySubscriptionCallback
155156
}
156157

157158
void dispatch(
158-
std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
159+
std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
159160
{
160161
TRACEPOINT(callback_start, (const void *)this, false);
161162
if (shared_ptr_callback_) {
@@ -181,7 +182,7 @@ class AnySubscriptionCallback
181182
}
182183

183184
void dispatch_intra_process(
184-
ConstMessageSharedPtr message, const rmw_message_info_t & message_info)
185+
ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
185186
{
186187
TRACEPOINT(callback_start, (const void *)this, true);
187188
if (const_shared_ptr_callback_) {
@@ -204,7 +205,7 @@ class AnySubscriptionCallback
204205
}
205206

206207
void dispatch_intra_process(
207-
MessageUniquePtr message, const rmw_message_info_t & message_info)
208+
MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
208209
{
209210
TRACEPOINT(callback_start, (const void *)this, true);
210211
if (shared_ptr_callback_) {

‎rclcpp/include/rclcpp/client.hpp‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef RCLCPP__CLIENT_HPP_
1616
#define RCLCPP__CLIENT_HPP_
1717

18+
#include <atomic>
1819
#include <future>
1920
#include <map>
2021
#include <memory>
@@ -62,6 +63,27 @@ class ClientBase
6263
RCLCPP_PUBLIC
6364
virtual ~ClientBase();
6465

66+
/// Take the next response for this client as a type erased pointer.
67+
/**
68+
* The type erased pointer allows for this method to be used in a type
69+
* agnostic way along with ClientBase::create_response(),
70+
* ClientBase::create_request_header(), and ClientBase::handle_response().
71+
* The typed version of this can be used if the Service type is known,
72+
* \sa Client::take_response().
73+
*
74+
* \param[out] response_out The type erased pointer to a Service Response into
75+
* which the middleware will copy the response being taken.
76+
* \param[out] request_header_out The request header to be filled by the
77+
* middleware when taking, and which can be used to associte the response
78+
* to a specific request.
79+
* \returns true if the response was taken, otherwise false.
80+
* \throws rclcpp::exceptions::RCLError based exceptions if the underlying
81+
* rcl function fail.
82+
*/
83+
RCLCPP_PUBLIC
84+
bool
85+
take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out);
86+
6587
RCLCPP_PUBLIC
6688
const char *
6789
get_service_name() const;
@@ -93,6 +115,20 @@ class ClientBase
93115
virtual void handle_response(
94116
std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
95117

118+
/// Exchange the "in use by wait set" state for this client.
119+
/**
120+
* This is used to ensure this client is not used by multiple
121+
* wait sets at the same time.
122+
*
123+
* \param[in] in_use_state the new state to exchange into the state, true
124+
* indicates it is now in use by a wait set, and false is that it is no
125+
* longer in use by a wait set.
126+
* \returns the previous state.
127+
*/
128+
RCLCPP_PUBLIC
129+
bool
130+
exchange_in_use_by_wait_set_state(bool in_use_state);
131+
96132
protected:
97133
RCLCPP_DISABLE_COPY(ClientBase)
98134

@@ -113,6 +149,8 @@ class ClientBase
113149
std::shared_ptr<rclcpp::Context> context_;
114150

115151
std::shared_ptr<rcl_client_t> client_handle_;
152+
153+
std::atomic<bool> in_use_by_wait_set_{false};
116154
};
117155

118156
template<typename ServiceT>
@@ -171,6 +209,25 @@ class Client : public ClientBase
171209
{
172210
}
173211

212+
/// Take the next response for this client.
213+
/**
214+
* \sa ClientBase::take_type_erased_response().
215+
*
216+
* \param[out] response_out The reference to a Service Response into
217+
* which the middleware will copy the response being taken.
218+
* \param[out] request_header_out The request header to be filled by the
219+
* middleware when taking, and which can be used to associte the response
220+
* to a specific request.
221+
* \returns true if the response was taken, otherwise false.
222+
* \throws rclcpp::exceptions::RCLError based exceptions if the underlying
223+
* rcl function fail.
224+
*/
225+
bool
226+
take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out)
227+
{
228+
return this->take_type_erased_response(&response_out, request_header_out);
229+
}
230+
174231
std::shared_ptr<void>
175232
create_response() override
176233
{

‎rclcpp/include/rclcpp/context.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -159,7 +159,7 @@ class Context : public std::enable_shared_from_this<Context>
159159
*
160160
* \param[in] reason the description of why shutdown happened
161161
* \return true if shutdown was successful, false if context was already shutdown
162-
* \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails
162+
* \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
163163
*/
164164
RCLCPP_PUBLIC
165165
virtual

‎rclcpp/include/rclcpp/guard_condition.hpp‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
// Copyright 2020 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RCLCPP__GUARD_CONDITION_HPP_
16+
#define RCLCPP__GUARD_CONDITION_HPP_
17+
18+
#include <atomic>
19+
20+
#include "rcl/guard_condition.h"
21+
22+
#include "rclcpp/context.hpp"
23+
#include "rclcpp/contexts/default_context.hpp"
24+
#include "rclcpp/macros.hpp"
25+
#include "rclcpp/visibility_control.hpp"
26+
27+
namespace rclcpp
28+
{
29+
30+
/// A condition that can be waited on in a single wait set and asynchronously triggered.
31+
class GuardCondition
32+
{
33+
public:
34+
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition)
35+
36+
// TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
37+
/// Construct the guard condition, optionally specifying which Context to use.
38+
/**
39+
* \param[in] context Optional custom context to be used.
40+
* Defaults to using the global default context singleton.
41+
* Shared ownership of the context is held with the guard condition until
42+
* destruction.
43+
* \throws std::invalid_argument if the context is nullptr.
44+
* \throws rclcpp::exceptions::RCLError based exceptions when underlying
45+
* rcl functions fail.
46+
*/
47+
RCLCPP_PUBLIC
48+
explicit GuardCondition(
49+
rclcpp::Context::SharedPtr context =
50+
rclcpp::contexts::default_context::get_global_default_context());
51+
52+
RCLCPP_PUBLIC
53+
virtual
54+
~GuardCondition();
55+
56+
/// Return the context used when creating this guard condition.
57+
RCLCPP_PUBLIC
58+
rclcpp::Context::SharedPtr
59+
get_context() const;
60+
61+
/// Return the underlying rcl guard condition structure.
62+
RCLCPP_PUBLIC
63+
const rcl_guard_condition_t &
64+
get_rcl_guard_condition() const;
65+
66+
/// Notify the wait set waiting on this condition, if any, that the condition had been met.
67+
/**
68+
* This function is thread-safe, and may be called concurrently with waiting
69+
* on this guard condition in a wait set.
70+
*
71+
* \throws rclcpp::exceptions::RCLError based exceptions when underlying
72+
* rcl functions fail.
73+
*/
74+
RCLCPP_PUBLIC
75+
void
76+
trigger();
77+
78+
/// Exchange the "in use by wait set" state for this guard condition.
79+
/**
80+
* This is used to ensure this guard condition is not used by multiple
81+
* wait sets at the same time.
82+
*
83+
* \param[in] in_use_state the new state to exchange into the state, true
84+
* indicates it is now in use by a wait set, and false is that it is no
85+
* longer in use by a wait set.
86+
* \returns the previous state.
87+
*/
88+
RCLCPP_PUBLIC
89+
bool
90+
exchange_in_use_by_wait_set_state(bool in_use_state);
91+
92+
protected:
93+
rclcpp::Context::SharedPtr context_;
94+
rcl_guard_condition_t rcl_guard_condition_;
95+
std::atomic<bool> in_use_by_wait_set_{false};
96+
};
97+
98+
} // namespace rclcpp
99+
100+
#endif // RCLCPP__GUARD_CONDITION_HPP_

‎rclcpp/include/rclcpp/message_info.hpp‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
// Copyright 2020 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RCLCPP__MESSAGE_INFO_HPP_
16+
#define RCLCPP__MESSAGE_INFO_HPP_
17+
18+
#include "rmw/types.h"
19+
20+
#include "rclcpp/visibility_control.hpp"
21+
22+
namespace rclcpp
23+
{
24+
25+
/// Additional meta data about messages taken from subscriptions.
26+
class RCLCPP_PUBLIC MessageInfo
27+
{
28+
public:
29+
/// Default empty constructor.
30+
MessageInfo() = default;
31+
32+
/// Conversion constructor, which is intentionally not marked as explicit.
33+
// cppcheck-suppress noExplicitConstructor
34+
MessageInfo(const rmw_message_info_t & rmw_message_info); // NOLINT(runtime/explicit)
35+
36+
virtual ~MessageInfo();
37+
38+
/// Return the message info as the underlying rmw message info type.
39+
const rmw_message_info_t &
40+
get_rmw_message_info() const;
41+
42+
/// Return the message info as the underlying rmw message info type.
43+
rmw_message_info_t &
44+
get_rmw_message_info();
45+
46+
private:
47+
rmw_message_info_t rmw_message_info_;
48+
};
49+
50+
} // namespace rclcpp
51+
52+
#endif // RCLCPP__MESSAGE_INFO_HPP_

‎rclcpp/include/rclcpp/rclcpp.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -143,6 +143,7 @@
143143
#include <memory>
144144

145145
#include "rclcpp/executors.hpp"
146+
#include "rclcpp/guard_condition.hpp"
146147
#include "rclcpp/logging.hpp"
147148
#include "rclcpp/node.hpp"
148149
#include "rclcpp/parameter.hpp"
@@ -152,6 +153,7 @@
152153
#include "rclcpp/time.hpp"
153154
#include "rclcpp/utilities.hpp"
154155
#include "rclcpp/visibility_control.hpp"
156+
#include "rclcpp/wait_set.hpp"
155157
#include "rclcpp/waitable.hpp"
156158

157159
#endif // RCLCPP__RCLCPP_HPP_

0 commit comments

Comments
 (0)