Skip to content

Commit 8fc9ca3

Browse files
hidmichidmic
authored
Ensure compliant publisher API. (#414)
Signed-off-by: Michel Hidalgo <[email protected]>
1 parent ece181f commit 8fc9ca3

File tree

7 files changed

+204
-157
lines changed

7 files changed

+204
-157
lines changed

‎rmw_fastrtps_cpp/src/publisher.cpp‎

Lines changed: 51 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,9 @@
2121
#include "rmw/allocators.h"
2222
#include "rmw/error_handling.h"
2323
#include "rmw/rmw.h"
24+
#include "rmw/validate_full_topic_name.h"
25+
26+
#include "rcpputils/scope_exit.hpp"
2427

2528
#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp"
2629
#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
@@ -49,22 +52,30 @@ rmw_fastrtps_cpp::create_publisher(
4952
bool keyed,
5053
bool create_publisher_listener)
5154
{
52-
if (!participant_info) {
53-
RMW_SET_ERROR_MSG("participant_info is null");
54-
return nullptr;
55-
}
56-
if (!topic_name || strlen(topic_name) == 0) {
57-
RMW_SET_ERROR_MSG("publisher topic is null or empty string");
55+
RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr);
56+
RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr);
57+
RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr);
58+
if (0 == strlen(topic_name)) {
59+
RMW_SET_ERROR_MSG("topic_name argument is an empty string");
5860
return nullptr;
5961
}
60-
if (!qos_policies) {
61-
RMW_SET_ERROR_MSG("qos_policies is null");
62-
return nullptr;
63-
}
64-
if (!publisher_options) {
65-
RMW_SET_ERROR_MSG("publisher_options is null");
66-
return nullptr;
62+
RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr);
63+
if (!qos_policies->avoid_ros_namespace_conventions) {
64+
int validation_result = RMW_TOPIC_VALID;
65+
rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr);
66+
if (RMW_RET_OK != ret) {
67+
return nullptr;
68+
}
69+
if (RMW_TOPIC_VALID != validation_result) {
70+
const char * reason = rmw_full_topic_name_validation_result_string(validation_result);
71+
RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason);
72+
return nullptr;
73+
}
6774
}
75+
RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr);
76+
77+
Participant * participant = participant_info->participant;
78+
RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr);
6879

6980
const rosidl_message_type_support_t * type_support = get_message_typesupport_handle(
7081
type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C);
@@ -93,23 +104,30 @@ rmw_fastrtps_cpp::create_publisher(
93104
RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo");
94105
return nullptr;
95106
}
96-
107+
auto cleanup_info = rcpputils::make_scope_exit(
108+
[info, participant]() {
109+
if (info->type_support_) {
110+
_unregister_type(participant, info->type_support_);
111+
}
112+
delete info->listener_;
113+
delete info;
114+
});
97115
info->typesupport_identifier_ = type_support->typesupport_identifier;
98116
info->type_support_impl_ = type_support->data;
99117

100118
auto callbacks = static_cast<const message_type_support_callbacks_t *>(type_support->data);
101119
std::string type_name = _create_type_name(callbacks);
102120
if (
103121
!Domain::getRegisteredType(
104-
participant_info->participant, type_name.c_str(),
122+
participant, type_name.c_str(),
105123
reinterpret_cast<TopicDataType **>(&info->type_support_)))
106124
{
107125
info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks);
108126
if (!info->type_support_) {
109-
RMW_SET_ERROR_MSG("Failed to allocate MessageTypeSupport");
110-
goto fail;
127+
RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport");
128+
return nullptr;
111129
}
112-
_register_type(participant_info->participant, info->type_support_);
130+
_register_type(participant, info->type_support_);
113131
}
114132

115133
if (!participant_info->leave_middleware_default_qos) {
@@ -124,26 +142,25 @@ rmw_fastrtps_cpp::create_publisher(
124142
publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name);
125143

126144
if (!get_datawriter_qos(*qos_policies, publisherParam)) {
127-
RMW_SET_ERROR_MSG("failed to get datawriter qos");
128-
goto fail;
145+
return nullptr;
129146
}
130147

131148
info->listener_ = nullptr;
132149
if (create_publisher_listener) {
133150
info->listener_ = new (std::nothrow) PubListener(info);
134151
if (!info->listener_) {
135152
RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener");
136-
goto fail;
153+
return nullptr;
137154
}
138155
}
139156

140157
info->publisher_ = Domain::createPublisher(
141-
participant_info->participant,
158+
participant,
142159
publisherParam,
143160
info->listener_);
144161
if (!info->publisher_) {
145162
RMW_SET_ERROR_MSG("create_publisher() could not create publisher");
146-
goto fail;
163+
return nullptr;
147164
}
148165

149166
info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
@@ -152,30 +169,27 @@ rmw_fastrtps_cpp::create_publisher(
152169
rmw_publisher = rmw_publisher_allocate();
153170
if (!rmw_publisher) {
154171
RMW_SET_ERROR_MSG("failed to allocate publisher");
155-
goto fail;
172+
return nullptr;
156173
}
174+
auto cleanup_publisher = rcpputils::make_scope_exit(
175+
[rmw_publisher]() {
176+
rmw_free(const_cast<char *>(rmw_publisher->topic_name));
177+
rmw_publisher_free(rmw_publisher);
178+
});
179+
157180
rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier;
158181
rmw_publisher->data = info;
159-
rmw_publisher->topic_name = static_cast<char *>(rmw_allocate(strlen(topic_name) + 1));
160182

183+
rmw_publisher->topic_name = static_cast<char *>(rmw_allocate(strlen(topic_name) + 1));
161184
if (!rmw_publisher->topic_name) {
162185
RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name");
163-
goto fail;
186+
return nullptr;
164187
}
165-
166188
memcpy(const_cast<char *>(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1);
167189

168190
rmw_publisher->options = *publisher_options;
169191

192+
cleanup_publisher.cancel();
193+
cleanup_info.cancel();
170194
return rmw_publisher;
171-
172-
fail:
173-
if (info) {
174-
delete info->type_support_;
175-
delete info->listener_;
176-
delete info;
177-
}
178-
rmw_publisher_free(rmw_publisher);
179-
180-
return nullptr;
181195
}

‎rmw_fastrtps_cpp/src/rmw_publisher.cpp‎

Lines changed: 32 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@
1919
#include "rmw/error_handling.h"
2020
#include "rmw/rmw.h"
2121

22+
#include "rmw/impl/cpp/macros.hpp"
23+
2224
#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
2325
#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp"
2426
#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
@@ -63,15 +65,12 @@ rmw_create_publisher(
6365
const rmw_qos_profile_t * qos_policies,
6466
const rmw_publisher_options_t * publisher_options)
6567
{
66-
if (!node) {
67-
RMW_SET_ERROR_MSG("node handle is null");
68-
return nullptr;
69-
}
70-
71-
if (node->implementation_identifier != eprosima_fastrtps_identifier) {
72-
RMW_SET_ERROR_MSG("node handle not from this implementation");
73-
return nullptr;
74-
}
68+
RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr);
69+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
70+
node,
71+
node->implementation_identifier,
72+
eprosima_fastrtps_identifier,
73+
return nullptr);
7574

7675
rmw_publisher_t * publisher = rmw_fastrtps_cpp::create_publisher(
7776
static_cast<CustomParticipantInfo *>(node->context->impl->participant_info),
@@ -101,8 +100,18 @@ rmw_create_publisher(
101100
static_cast<void *>(&msg),
102101
nullptr);
103102
if (RMW_RET_OK != rmw_ret) {
104-
rmw_fastrtps_shared_cpp::__rmw_destroy_publisher(
103+
rmw_error_state_t error_state = *rmw_get_error_state();
104+
rmw_reset_error();
105+
static_cast<void>(common_context->graph_cache.dissociate_writer(
106+
info->publisher_gid, common_context->gid, node->name, node->namespace_));
107+
rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_publisher(
105108
eprosima_fastrtps_identifier, node, publisher);
109+
if (RMW_RET_OK != rmw_ret) {
110+
RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str);
111+
RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n");
112+
rmw_reset_error();
113+
}
114+
rmw_set_error_state(error_state.message, error_state.file, error_state.line_number);
106115
return nullptr;
107116
}
108117
}
@@ -164,6 +173,19 @@ rmw_return_loaned_message_from_publisher(
164173
rmw_ret_t
165174
rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher)
166175
{
176+
RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
177+
RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT);
178+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
179+
node,
180+
node->implementation_identifier,
181+
eprosima_fastrtps_identifier,
182+
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
183+
RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
184+
publisher,
185+
publisher->implementation_identifier,
186+
eprosima_fastrtps_identifier,
187+
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
188+
167189
return rmw_fastrtps_shared_cpp::__rmw_destroy_publisher(
168190
eprosima_fastrtps_identifier, node, publisher);
169191
}

‎rmw_fastrtps_dynamic_cpp/src/publisher.cpp‎

Lines changed: 50 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,9 @@
1818
#include "rmw/allocators.h"
1919
#include "rmw/error_handling.h"
2020
#include "rmw/rmw.h"
21+
#include "rmw/validate_full_topic_name.h"
22+
23+
#include "rcpputils/scope_exit.hpp"
2124

2225
#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
2326
#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp"
@@ -51,31 +54,30 @@ rmw_fastrtps_dynamic_cpp::create_publisher(
5154
(void)keyed;
5255
(void)create_publisher_listener;
5356

54-
if (!participant_info) {
55-
RMW_SET_ERROR_MSG("participant_info is null");
56-
return nullptr;
57-
}
58-
59-
if (!topic_name || strlen(topic_name) == 0) {
60-
RMW_SET_ERROR_MSG("publisher topic is null or empty string");
61-
return nullptr;
62-
}
63-
64-
if (!qos_policies) {
65-
RMW_SET_ERROR_MSG("qos_policies is null");
57+
RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr);
58+
RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr);
59+
RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr);
60+
if (0 == strlen(topic_name)) {
61+
RMW_SET_ERROR_MSG("topic_name argument is an empty string");
6662
return nullptr;
6763
}
68-
69-
if (!publisher_options) {
70-
RMW_SET_ERROR_MSG("publisher_options is null");
71-
return nullptr;
64+
RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr);
65+
if (!qos_policies->avoid_ros_namespace_conventions) {
66+
int validation_result = RMW_TOPIC_VALID;
67+
rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr);
68+
if (RMW_RET_OK != ret) {
69+
return nullptr;
70+
}
71+
if (RMW_TOPIC_VALID != validation_result) {
72+
const char * reason = rmw_full_topic_name_validation_result_string(validation_result);
73+
RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason);
74+
return nullptr;
75+
}
7276
}
77+
RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr);
7378

7479
Participant * participant = participant_info->participant;
75-
if (!participant) {
76-
RMW_SET_ERROR_MSG("participant handle is null");
77-
return nullptr;
78-
}
80+
RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr);
7981

8082
const rosidl_message_type_support_t * type_support = get_message_typesupport_handle(
8183
type_supports, rosidl_typesupport_introspection_c__identifier);
@@ -104,14 +106,25 @@ rmw_fastrtps_dynamic_cpp::create_publisher(
104106
RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo");
105107
return nullptr;
106108
}
109+
auto cleanup_info = rcpputils::make_scope_exit(
110+
[info, participant]() {
111+
if (info->type_support_) {
112+
_unregister_type(participant, info->type_support_);
113+
}
114+
delete info->listener_;
115+
delete info;
116+
});
107117

108118
TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance();
109119
auto type_impl = type_registry.get_message_type_support(type_support);
110120
if (!type_impl) {
111-
delete info;
112121
RMW_SET_ERROR_MSG("failed to allocate type support");
113122
return nullptr;
114123
}
124+
auto return_type_support = rcpputils::make_scope_exit(
125+
[&type_registry, type_support]() {
126+
type_registry.return_message_type_support(type_support);
127+
});
115128

116129
info->typesupport_identifier_ = type_support->typesupport_identifier;
117130
info->type_support_impl_ = type_impl;
@@ -126,7 +139,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher(
126139
info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl);
127140
if (!info->type_support_) {
128141
RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy");
129-
goto fail;
142+
return nullptr;
130143
}
131144
_register_type(participant, info->type_support_);
132145
}
@@ -150,20 +163,19 @@ rmw_fastrtps_dynamic_cpp::create_publisher(
150163
// publisherParam.throughputController = throughputController;
151164

152165
if (!get_datawriter_qos(*qos_policies, publisherParam)) {
153-
RMW_SET_ERROR_MSG("failed to get datawriter qos");
154-
goto fail;
166+
return nullptr;
155167
}
156168

157169
info->listener_ = new (std::nothrow) PubListener(info);
158170
if (!info->listener_) {
159171
RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener");
160-
goto fail;
172+
return nullptr;
161173
}
162174

163175
info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_);
164176
if (!info->publisher_) {
165177
RMW_SET_ERROR_MSG("create_publisher() could not create publisher");
166-
goto fail;
178+
return nullptr;
167179
}
168180

169181
info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
@@ -172,32 +184,29 @@ rmw_fastrtps_dynamic_cpp::create_publisher(
172184
rmw_publisher = rmw_publisher_allocate();
173185
if (!rmw_publisher) {
174186
RMW_SET_ERROR_MSG("failed to allocate publisher");
175-
goto fail;
187+
return nullptr;
176188
}
189+
auto cleanup_publisher = rcpputils::make_scope_exit(
190+
[rmw_publisher]() {
191+
rmw_free(const_cast<char *>(rmw_publisher->topic_name));
192+
rmw_publisher_free(rmw_publisher);
193+
});
194+
177195
rmw_publisher->can_loan_messages = false;
178196
rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier;
179197
rmw_publisher->data = info;
180-
rmw_publisher->topic_name = static_cast<char *>(rmw_allocate(strlen(topic_name) + 1));
181198

199+
rmw_publisher->topic_name = static_cast<char *>(rmw_allocate(strlen(topic_name) + 1));
182200
if (!rmw_publisher->topic_name) {
183201
RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name");
184-
goto fail;
202+
return nullptr;
185203
}
186-
187204
memcpy(const_cast<char *>(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1);
188205

189206
rmw_publisher->options = *publisher_options;
190207

208+
cleanup_publisher.cancel();
209+
cleanup_info.cancel();
210+
return_type_support.cancel();
191211
return rmw_publisher;
192-
193-
fail:
194-
if (info) {
195-
delete info->type_support_;
196-
delete info->listener_;
197-
delete info;
198-
}
199-
type_registry.return_message_type_support(type_support);
200-
rmw_publisher_free(rmw_publisher);
201-
202-
return nullptr;
203212
}

0 commit comments

Comments
 (0)