diff --git a/include/rosx_introspection/ros_utils/ros2_helpers.hpp b/include/rosx_introspection/ros_utils/ros2_helpers.hpp index 1d860d4..b6de083 100644 --- a/include/rosx_introspection/ros_utils/ros2_helpers.hpp +++ b/include/rosx_introspection/ros_utils/ros2_helpers.hpp @@ -72,7 +72,7 @@ inline std::vector BuildMessageBuffer(const T& msg, } template -inline T BufferToMessage(const std::vector& buffer) +inline T BufferToMessage(const void* bufferPtr, size_t bufferSize) { // get the type name of the ROS message with traits const std::string topic_type = rosidl_generator_traits::name(); @@ -82,13 +82,19 @@ inline T BufferToMessage(const std::vector& buffer) rosbag2_cpp::get_typesupport_handle(topic_type, ts_identifier, ts_library); rmw_serialized_message_t serialized_msg; - serialized_msg.buffer_capacity = buffer.size(); - serialized_msg.buffer_length = buffer.size(); - serialized_msg.buffer = const_cast(buffer.data()); + serialized_msg.buffer_capacity = bufferSize; + serialized_msg.buffer_length = bufferSize; + serialized_msg.buffer = const_cast(reinterpret_cast(bufferPtr)); T message; auto res = rmw_deserialize(&serialized_msg, type_support, &message); return message; } +template +inline T BufferToMessage(const std::vector& buffer) +{ + return BufferToMessage(buffer.data(), buffer.size()); +} + } // namespace RosMsgParser diff --git a/test/test_ros2.cpp b/test/test_ros2.cpp index 406219b..0420794 100644 --- a/test/test_ros2.cpp +++ b/test/test_ros2.cpp @@ -120,12 +120,9 @@ TEST(ParseROS2, JointState_JSON) ROS2_Serializer serializer; parser.serializeFromJson(json_text, &serializer); - std::vector buffer_out(serializer.getBufferSize()); - memcpy(buffer_out.data(), serializer.getBufferData(), buffer_out.size()); - - auto joint_state_out = BufferToMessage(buffer_out); - - ASSERT_EQ(buffer_in.size(), buffer_out.size()); + auto joint_state_out = BufferToMessage( + serializer.getBufferData(), serializer.getBufferSize() + ); ASSERT_EQ(joint_state.header.frame_id, joint_state_out.header.frame_id); ASSERT_EQ(joint_state.header.stamp.sec, joint_state_out.header.stamp.sec); @@ -156,10 +153,9 @@ TEST(ParseROS2, JointState_JSON_Omitted) ROS2_Serializer serializer; parser.serializeFromJson(joint_state_json, &serializer); - std::vector buffer_out(serializer.getBufferSize()); - memcpy(buffer_out.data(), serializer.getBufferData(), buffer_out.size()); - - auto joint_state_out = BufferToMessage(buffer_out); + auto joint_state_out = BufferToMessage( + serializer.getBufferData(), serializer.getBufferSize() + ); ASSERT_EQ("", joint_state_out.header.frame_id); // default ASSERT_EQ(1234, joint_state_out.header.stamp.sec); @@ -211,12 +207,9 @@ TEST(ParseROS2, PoseStamped_JSON) ROS2_Serializer serializer; parser.serializeFromJson(json_text, &serializer); - std::vector buffer_out(serializer.getBufferSize()); - memcpy(buffer_out.data(), serializer.getBufferData(), buffer_out.size()); - - auto pose_stamped_out = BufferToMessage(buffer_out); - - ASSERT_EQ(buffer_in.size(), buffer_out.size()); + auto pose_stamped_out = BufferToMessage( + serializer.getBufferData(), serializer.getBufferSize() + ); ASSERT_EQ(pose_stamped.header.frame_id, pose_stamped_out.header.frame_id); ASSERT_EQ(pose_stamped.header.stamp.sec, pose_stamped_out.header.stamp.sec); @@ -246,10 +239,9 @@ TEST(ParseROS2, PoseStamped_JSON_Omitted) ROS2_Serializer serializer; parser.serializeFromJson(pose_stamped_json, &serializer); - std::vector buffer_out(serializer.getBufferSize()); - memcpy(buffer_out.data(), serializer.getBufferData(), buffer_out.size()); - - auto pose_stamped_out = BufferToMessage(buffer_out); + auto pose_stamped_out = BufferToMessage( + serializer.getBufferData(), serializer.getBufferSize() + ); ASSERT_EQ("base", pose_stamped_out.header.frame_id); ASSERT_EQ(1234, pose_stamped_out.header.stamp.sec); @@ -265,3 +257,34 @@ TEST(ParseROS2, PoseStamped_JSON_Omitted) ASSERT_EQ(0, pose_stamped_out.pose.orientation.z); ASSERT_EQ(0, pose_stamped_out.pose.orientation.w); } + +TEST(ParseROS2, Duration) +{ + const char* durationA_json = R"({"sec":123,"nanosec":456})"; + + const std::string topic_type = "builtin_interfaces/Duration"; + Parser parser("", ROSType(topic_type), GetMessageDefinition(topic_type)); + + // We omitted the effort field and the header.frame_id + ROS2_Serializer serializer; + parser.serializeFromJson(durationA_json, &serializer); + + auto durationA = BufferToMessage( + serializer.getBufferData(), serializer.getBufferSize() + ); + + ASSERT_EQ(durationA.sec, 123); + ASSERT_EQ(durationA.nanosec, 456); + //------------------------------ + const char* durationB_json = R"({"sec":1,"nanosec":234})"; + serializer.reset(); + parser.serializeFromJson(durationB_json, &serializer); + + auto durationB = BufferToMessage( + serializer.getBufferData(), serializer.getBufferSize() + ); + + ASSERT_EQ(durationB.sec, 1); + ASSERT_EQ(durationB.nanosec, 234); + +} \ No newline at end of file