forked from ros2/rclcpp
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathpublisher_base.hpp
More file actions
231 lines (195 loc) · 6.79 KB
/
publisher_base.hpp
File metadata and controls
231 lines (195 loc) · 6.79 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
// Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__PUBLISHER_BASE_HPP_
#define RCLCPP__PUBLISHER_BASE_HPP_
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <vector>
#include "rcl/publisher.h"
#include "rclcpp/macros.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/qos_event.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
// Forward declaration is used for friend statement.
namespace node_interfaces
{
class NodeBaseInterface;
class NodeTopicsInterface;
} // namespace node_interfaces
namespace experimental
{
/**
* IntraProcessManager is forward declared here, avoiding a circular inclusion between
* `intra_process_manager.hpp` and `publisher_base.hpp`.
*/
class IntraProcessManager;
} // namespace experimental
class PublisherBase : public std::enable_shared_from_this<PublisherBase>
{
friend ::rclcpp::node_interfaces::NodeTopicsInterface;
public:
RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase)
/// Default constructor.
/**
* Typically, a publisher is not created through this method, but instead is created through a
* call to `Node::create_publisher`.
* \param[in] node_base A pointer to the NodeBaseInterface for the parent node.
* \param[in] topic The topic that this publisher publishes on.
* \param[in] type_support The type support structure for the type to be published.
* \param[in] publisher_options QoS settings for this publisher.
*/
RCLCPP_PUBLIC
PublisherBase(
rclcpp::node_interfaces::NodeBaseInterface * node_base,
const std::string & topic,
const rosidl_message_type_support_t & type_support,
const rcl_publisher_options_t & publisher_options);
RCLCPP_PUBLIC
virtual ~PublisherBase();
/// Get the topic that this publisher publishes on.
/** \return The topic name. */
RCLCPP_PUBLIC
const char *
get_topic_name() const;
/// Get the queue size for this publisher.
/** \return The queue size. */
RCLCPP_PUBLIC
size_t
get_queue_size() const;
/// Get the global identifier for this publisher (used in rmw and by DDS).
/** \return The gid. */
RCLCPP_PUBLIC
const rmw_gid_t &
get_gid() const;
/// Get the rcl publisher handle.
/** \return The rcl publisher handle. */
RCLCPP_PUBLIC
rcl_publisher_t *
get_publisher_handle();
/// Get the rcl publisher handle.
/** \return The rcl publisher handle. */
RCLCPP_PUBLIC
const rcl_publisher_t *
get_publisher_handle() const;
/// Get all the QoS event handlers associated with this publisher.
/** \return The vector of QoS event handlers. */
RCLCPP_PUBLIC
const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
get_event_handlers() const;
/// Get subscription count
/** \return The number of subscriptions. */
RCLCPP_PUBLIC
size_t
get_subscription_count() const;
/// Get intraprocess subscription count
/** \return The number of intraprocess subscriptions. */
RCLCPP_PUBLIC
size_t
get_intra_process_subscription_count() const;
/// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC).
/**
* If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator
* of this publisher may manually call `assert_liveliness` at some point in time to signal to the
* rest of the system that this Node is still alive.
*
* \return `true` if the liveliness was asserted successfully, otherwise `false`
*/
RCLCPP_PUBLIC
RCUTILS_WARN_UNUSED
bool
assert_liveliness() const;
/// Get the actual QoS settings, after the defaults have been determined.
/**
* The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
* can only be resolved after the creation of the publisher, and it
* depends on the underlying rmw implementation.
* If the underlying setting in use can't be represented in ROS terms,
* it will be set to RMW_QOS_POLICY_*_UNKNOWN.
* May throw runtime_error when an unexpected error occurs.
*
* \return The actual qos settings.
*/
RCLCPP_PUBLIC
rclcpp::QoS
get_actual_qos() const;
/// Check if publisher instance can loan messages.
/**
* Depending on the middleware and the message type, this will return true if the middleware
* can allocate a ROS message instance.
*/
RCLCPP_PUBLIC
bool
can_loan_messages() const;
/// Compare this publisher to a gid.
/**
* Note that this function calls the next function.
* \param[in] gid Reference to a gid.
* \return True if the publisher's gid matches the input.
*/
RCLCPP_PUBLIC
bool
operator==(const rmw_gid_t & gid) const;
/// Compare this publisher to a pointer gid.
/**
* A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal.
* \param[in] gid A pointer to a gid.
* \return True if this publisher's gid matches the input.
*/
RCLCPP_PUBLIC
bool
operator==(const rmw_gid_t * gid) const;
using IntraProcessManagerSharedPtr =
std::shared_ptr<rclcpp::experimental::IntraProcessManager>;
/// Implementation utility function used to setup intra process publishing after creation.
RCLCPP_PUBLIC
void
setup_intra_process(
uint64_t intra_process_publisher_id,
IntraProcessManagerSharedPtr ipm);
protected:
template<typename EventCallbackT>
void
add_event_handler(
const EventCallbackT & callback,
const rcl_publisher_event_type_t event_type)
{
auto handler = std::make_shared<QOSEventHandler<EventCallbackT>>(
callback,
rcl_publisher_event_init,
&publisher_handle_,
event_type);
event_handlers_.emplace_back(handler);
}
RCLCPP_PUBLIC
void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const;
std::shared_ptr<rcl_node_t> rcl_node_handle_;
rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
using IntraProcessManagerWeakPtr =
std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
bool intra_process_is_enabled_;
IntraProcessManagerWeakPtr weak_ipm_;
uint64_t intra_process_publisher_id_;
rmw_gid_t rmw_gid_;
};
} // namespace rclcpp
#endif // RCLCPP__PUBLISHER_BASE_HPP_