Skip to content

Commit d6bf983

Browse files
Karsten1987Karsten1987
committed
make serialized message a class
Signed-off-by: Karsten Knese <karsten@openrobotics.org>
1 parent 1cd811a commit d6bf983

8 files changed

Lines changed: 309 additions & 125 deletions

File tree

‎rclcpp/CMakeLists.txt‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,8 @@ set(${PROJECT_NAME}_SRCS
7777
src/rclcpp/publisher_base.cpp
7878
src/rclcpp/qos.cpp
7979
src/rclcpp/qos_event.cpp
80+
src/rclcpp/serialization.cpp
81+
src/rclcpp/serialized_message.cpp
8082
src/rclcpp/service.cpp
8183
src/rclcpp/signal_handler.cpp
8284
src/rclcpp/subscription_base.cpp
@@ -405,6 +407,15 @@ if(BUILD_TESTING)
405407
${PROJECT_NAME}
406408
)
407409
endif()
410+
ament_add_gtest(test_serialized_message test/test_serialized_message.cpp)
411+
if(TARGET test_serialized_message)
412+
ament_target_dependencies(test_serialized_message
413+
test_msgs
414+
)
415+
target_link_libraries(test_serialized_message
416+
${PROJECT_NAME}
417+
)
418+
endif()
408419
ament_add_gtest(test_service test/test_service.cpp)
409420
if(TARGET test_service)
410421
ament_target_dependencies(test_service

‎rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -170,15 +170,15 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
170170
msg_info.from_intra_process = true;
171171

172172
ConstMessageSharedPtr msg = buffer_->consume_shared();
173-
auto serialized_msg =
174-
serializer_->serialize_message(reinterpret_cast<const void *>(msg.get()));
173+
auto serialized_msg = std::make_shared<rclcpp::SerializedMessage>();
174+
serializer_->serialize_message(reinterpret_cast<const void *>(msg.get()), serialized_msg.get());
175175

176-
if (nullptr == serialized_msg) {
176+
if (0u == serialized_msg->buffer_length) {
177177
throw std::runtime_error("Subscription intra-process could not serialize message");
178178
}
179179

180180
if (any_callback_.use_take_shared_method()) {
181-
any_callback_.dispatch_intra_process(serialized_msg, msg_info);
181+
any_callback_.dispatch_intra_process(std::static_pointer_cast<rcl_serialized_message_t>(serialized_msg), msg_info);
182182
} else {
183183
throw std::runtime_error(
184184
"Subscription intra-process for serialized "
@@ -243,8 +243,8 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
243243
throw std::runtime_error("Subscription intra-process can't handle unserialized messages");
244244
}
245245

246-
ConstMessageSharedPtr serialized_container = buffer_->consume_shared();
247-
if (nullptr == serialized_container) {
246+
ConstMessageSharedPtr serialized_message = buffer_->consume_shared();
247+
if (nullptr == serialized_message) {
248248
throw std::runtime_error("Subscription intra-process could not get serialized message");
249249
}
250250

@@ -254,13 +254,13 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
254254
if (any_callback_.use_take_shared_method()) {
255255
CallbackMessageSharedPtr msg = construct_unique();
256256
serializer_->deserialize_message(
257-
serialized_container.get(),
257+
serialized_message.get(),
258258
reinterpret_cast<void *>(msg.get()));
259259
any_callback_.dispatch_intra_process(msg, msg_info);
260260
} else {
261261
CallbackMessageUniquePtr msg = construct_unique();
262262
serializer_->deserialize_message(
263-
serialized_container.get(),
263+
serialized_message.get(),
264264
reinterpret_cast<void *>(msg.get()));
265265
any_callback_.dispatch_intra_process(std::move(msg), msg_info);
266266
}

‎rclcpp/include/rclcpp/serialization.hpp‎

Lines changed: 11 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -15,16 +15,16 @@
1515
#ifndef RCLCPP__SERIALIZATION_HPP_
1616
#define RCLCPP__SERIALIZATION_HPP_
1717

18-
#include <rmw/rmw.h>
19-
2018
#include <memory>
2119
#include <string>
2220

23-
#include "rcl/error_handling.h"
21+
#include "rosidl_runtime_c/message_type_support_struct.h"
2422

2523
namespace rclcpp
2624
{
2725

26+
class SerializedMessage;
27+
2828
/// Interface to (de)serialize a message
2929
class SerializationBase
3030
{
@@ -35,87 +35,32 @@ class SerializationBase
3535
/**
3636
* \param[in] message The ROS2 message which is read and serialized by rmw.
3737
*/
38-
virtual std::shared_ptr<rcl_serialized_message_t> serialize_message(const void * message) = 0;
38+
virtual void serialize_message(
39+
const void * ros_message, SerializedMessage * serialized_message) const = 0;
3940

4041
/// Deserialize a serialized stream to a ROS message
4142
/**
4243
* \param[in] serialized_message The serialized message to be converted to ROS2 by rmw.
4344
* \param[out] message The deserialized ROS2 message.
4445
*/
4546
virtual void deserialize_message(
46-
const rcl_serialized_message_t * serialized_message,
47-
void * msg) = 0;
47+
const SerializedMessage * serialized_message, void * ros_message) const = 0;
4848
};
4949

5050
/// Default implementation to (de)serialize a message by using rmw_(de)serialize
5151
class Serialization : public SerializationBase
5252
{
5353
public:
54-
Serialization(
55-
const rosidl_message_type_support_t & type_support,
56-
const rcutils_allocator_t allocator = rcutils_get_default_allocator())
57-
: type_support_(type_support), rcutils_allocator_(allocator)
58-
{}
59-
60-
std::shared_ptr<rcl_serialized_message_t> serialize_message(const void * message) override
61-
{
62-
auto serialized_message = new rcl_serialized_message_t;
63-
*serialized_message = rmw_get_zero_initialized_serialized_message();
64-
const auto ret = rmw_serialized_message_init(serialized_message, 0, &rcutils_allocator_);
65-
if (ret != RCUTILS_RET_OK) {
66-
throw std::runtime_error(
67-
"Error allocating resources for serialized message: " +
68-
std::string(rcutils_get_error_string().str));
69-
}
70-
71-
if (nullptr == message) {
72-
delete serialized_message;
73-
throw std::runtime_error("Message is nullpointer while serialization.");
74-
}
75-
76-
const auto error = rmw_serialize(
77-
message,
78-
&type_support_,
79-
serialized_message);
80-
if (error != RCL_RET_OK) {
81-
delete serialized_message;
82-
throw std::runtime_error("Failed to serialize.");
83-
}
84-
85-
auto shared_serialized_msg = std::shared_ptr<rcl_serialized_message_t>(
86-
serialized_message,
87-
[](rcl_serialized_message_t * msg) {
88-
auto fini_ret = rmw_serialized_message_fini(msg);
89-
delete msg;
90-
if (fini_ret != RCL_RET_OK) {
91-
RCUTILS_LOG_ERROR_NAMED(
92-
"rclcpp",
93-
"Failed to destroy serialized message: %s", rcl_get_error_string().str);
94-
}
95-
});
96-
97-
return shared_serialized_msg;
98-
}
54+
Serialization(const rosidl_message_type_support_t & type_support);
9955

100-
void deserialize_message(const rcl_serialized_message_t * serialized_message, void * msg) override
101-
{
102-
if (nullptr == serialized_message ||
103-
serialized_message->buffer_capacity == 0 ||
104-
serialized_message->buffer_length == 0 ||
105-
!serialized_message->buffer)
106-
{
107-
throw std::runtime_error("Failed to deserialize nullptr serialized message.");
108-
}
56+
void serialize_message(
57+
const void * ros_message, SerializedMessage * serialized_message) const override;
10958

110-
const auto ret = rmw_deserialize(serialized_message, &type_support_, msg);
111-
if (ret != RMW_RET_OK) {
112-
throw std::runtime_error("Failed to deserialize serialized message.");
113-
}
114-
}
59+
void deserialize_message(
60+
const SerializedMessage * serialized_message, void * ros_message) const override;
11561

11662
private:
11763
rosidl_message_type_support_t type_support_;
118-
rcutils_allocator_t rcutils_allocator_;
11964
};
12065

12166
} // namespace rclcpp

‎rclcpp/include/rclcpp/serialized_message.hpp‎

Lines changed: 33 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -15,13 +15,8 @@
1515
#ifndef RCLCPP__SERIALIZED_MESSAGE_HPP_
1616
#define RCLCPP__SERIALIZED_MESSAGE_HPP_
1717

18-
#include <rclcpp/exceptions.hpp>
19-
20-
#include <cstring>
21-
22-
#include "rcutils/logging_macros.h"
23-
24-
#include "rmw/serialized_message.h"
18+
#include "rcl/allocator.h"
19+
#include "rcl/types.h"
2520

2621
namespace rclcpp
2722
{
@@ -30,49 +25,42 @@ namespace rclcpp
3025
class SerializedMessage : public rcl_serialized_message_t
3126
{
3227
public:
33-
SerializedMessage()
34-
: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message())
35-
{}
28+
/// Default constructor for a SerializedMessage
29+
/**
30+
* Default constructs a serialized message and initalizes its
31+
* capacity with 0.
32+
*
33+
* \param[in] allocator The allocator to be used for the initialzation.
34+
*/
35+
explicit SerializedMessage(
36+
const rcl_allocator_t & allocator = rcl_get_default_allocator());
37+
38+
/// Default constructor for a SerializedMessage
39+
/**
40+
* Default constructs a serialized message and initalizes its
41+
* capacity with 0.
42+
*
43+
* \param[in] initial_capacity The amount of memory to be allocated.
44+
* \param[in] allocator The allocator to be used for the initialzation.
45+
*/
46+
explicit SerializedMessage(
47+
size_t initial_capacity,
48+
const rcl_allocator_t & allocator = rcl_get_default_allocator());
3649

37-
explicit SerializedMessage(const SerializedMessage & serialized_message)
38-
: SerializedMessage(static_cast<const rcl_serialized_message_t>(serialized_message))
39-
{}
50+
/// Copy Constructor for a SerializedMessage
51+
explicit SerializedMessage(const SerializedMessage & serialized_message);
4052

41-
explicit SerializedMessage(const rcl_serialized_message_t & serialized_message)
42-
: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message())
43-
{
44-
const auto ret = rmw_serialized_message_init(
45-
this, serialized_message.buffer_length,
46-
&serialized_message.allocator);
47-
if (ret != RCL_RET_OK) {
48-
rclcpp::exceptions::throw_from_rcl_error(ret);
49-
}
53+
/// Copy Constructor for a SerializedMessage from a rcl_serialized_message_t
54+
explicit SerializedMessage(const rcl_serialized_message_t & serialized_message);
5055

51-
// do not call memcpy if the pointer is "static"
52-
if (buffer != serialized_message.buffer) {
53-
std::memcpy(buffer, serialized_message.buffer, serialized_message.buffer_length);
54-
}
55-
buffer_length = serialized_message.buffer_length;
56-
}
56+
/// Move Constructor for a SerializedMessage
57+
explicit SerializedMessage(SerializedMessage && serialized_message);
5758

58-
explicit SerializedMessage(rcl_serialized_message_t && msg)
59-
: rcl_serialized_message_t(msg)
60-
{
61-
// reset buffer to prevent double free
62-
msg = rmw_get_zero_initialized_serialized_message();
63-
}
59+
/// Move Constructor for a SerializedMessage from a rcl_serialized_message_t
60+
explicit SerializedMessage(rcl_serialized_message_t && serialized_message);
6461

65-
~SerializedMessage()
66-
{
67-
if (nullptr != buffer) {
68-
const auto fini_ret = rmw_serialized_message_fini(this);
69-
if (fini_ret != RCL_RET_OK) {
70-
RCUTILS_LOG_ERROR_NAMED(
71-
"rclcpp",
72-
"Failed to destroy serialized message: %s", rcl_get_error_string().str);
73-
}
74-
}
75-
}
62+
/// Destructor for a SerializedMessage
63+
~SerializedMessage();
7664
};
7765

7866
} // namespace rclcpp

‎rclcpp/include/rclcpp/subscription.hpp‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -177,9 +177,7 @@ class Subscription : public SubscriptionBase
177177
this->get_topic_name(), // important to get it by the fully-qualified name
178178
qos.get_rmw_qos_profile(),
179179
resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback),
180-
std::make_shared<rclcpp::Serialization>(
181-
type_support_handle,
182-
options.template to_rcl_subscription_options<CallbackMessageT>(qos).allocator)
180+
std::make_shared<rclcpp::Serialization>(type_support_handle)
183181
);
184182
TRACEPOINT(
185183
rclcpp_subscription_init,
@@ -216,9 +214,7 @@ class Subscription : public SubscriptionBase
216214
this->get_topic_name(), // important to get it by the fully-qualified name
217215
qos.get_rmw_qos_profile(),
218216
resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback),
219-
std::make_shared<rclcpp::Serialization>(
220-
type_support_handle,
221-
options.template to_rcl_subscription_options<CallbackMessageT>(qos).allocator)
217+
std::make_shared<rclcpp::Serialization>(type_support_handle)
222218
);
223219
TRACEPOINT(
224220
rclcpp_subscription_init,

‎rclcpp/src/rclcpp/serialization.cpp‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
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+
#include "rclcpp/serialization.hpp"
16+
17+
#include <memory>
18+
#include <string>
19+
20+
#include "rclcpp/exceptions.hpp"
21+
#include "rclcpp/serialized_message.hpp"
22+
23+
#include "rcpputils/asserts.hpp"
24+
25+
#include "rmw/rmw.h"
26+
27+
namespace rclcpp
28+
{
29+
30+
Serialization::Serialization(const rosidl_message_type_support_t & type_support)
31+
: type_support_(type_support)
32+
{}
33+
34+
void Serialization::serialize_message(
35+
const void * ros_message, SerializedMessage * serialized_message) const
36+
{
37+
rcpputils::check_true(ros_message != nullptr, "ROS message is nullpointer.");
38+
rcpputils::check_true(serialized_message != nullptr, "Serialized message is nullpointer.");
39+
40+
const auto ret = rmw_serialize(
41+
ros_message,
42+
&type_support_,
43+
serialized_message);
44+
if (ret != RMW_RET_OK) {
45+
rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to serialize ROS message.");
46+
}
47+
}
48+
49+
void
50+
Serialization::deserialize_message(
51+
const SerializedMessage * serialized_message, void * ros_message) const
52+
{
53+
rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer.");
54+
rcpputils::check_true(
55+
serialized_message->buffer_capacity != 0 &&
56+
serialized_message->buffer_length != 0 &&
57+
!serialized_message->buffer, "Serialized message is wrongly initialized.");
58+
{
59+
throw std::runtime_error("Failed to deserialize nullptr serialized message.");
60+
}
61+
62+
const auto ret = rmw_deserialize(serialized_message, &type_support_, ros_message);
63+
if (ret != RMW_RET_OK) {
64+
rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to deserialize ROS message.");
65+
}
66+
}
67+
68+
} // namespace rclcpp

0 commit comments

Comments
 (0)