├── .github └── workflows │ └── main.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── message_serialization │ ├── binary_serialization.h │ ├── eigen_yaml.h │ ├── geometry_msgs_yaml.h │ ├── sensor_msgs_yaml.h │ ├── serialize.h │ ├── shape_msgs_yaml.h │ ├── std_msgs_yaml.h │ └── trajectory_msgs_yaml.h ├── package.xml ├── scripts └── message_converter.py └── test ├── geometry_msgs_test.h ├── sensor_msgs_test.h ├── shape_msgs_test.h ├── std_msgs_test.h ├── trajectory_msgs_test.h ├── utest.cpp └── utilities.h /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | branches: 9 | - master 10 | schedule: 11 | - cron: '0 0 * * 0' 12 | 13 | jobs: 14 | industrial_ci: 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | env: 19 | - {CI_NAME: bionic, 20 | OS_NAME: ubuntu, 21 | OS_CODE_NAME: bionic, 22 | ROS_DISTRO: melodic, 23 | ROS_REPO: main, 24 | DOCKER_IMAGE: "ros:melodic"} 25 | - {CI_NAME: focal, 26 | OS_NAME: ubuntu, 27 | OS_CODE_NAME: focal, 28 | ROS_DISTRO: noetic, 29 | ROS_REPO: main, 30 | DOCKER_IMAGE: "ros:noetic"} 31 | runs-on: ubuntu-latest 32 | steps: 33 | - uses: actions/checkout@v2 34 | - uses: 'ros-industrial/industrial_ci@master' 35 | env: ${{matrix.env}} 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(message_serialization) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | eigen_conversions 8 | geometry_msgs 9 | sensor_msgs 10 | std_msgs 11 | roscpp_serialization 12 | trajectory_msgs 13 | ) 14 | 15 | find_package(yaml-cpp REQUIRED) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS 19 | include 20 | CATKIN_DEPENDS 21 | eigen_conversions 22 | geometry_msgs 23 | sensor_msgs 24 | std_msgs 25 | roscpp_serialization 26 | trajectory_msgs 27 | DEPENDS 28 | YAML_CPP 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | include_directories( 35 | include 36 | ${catkin_INCLUDE_DIRS} 37 | ${YAML_CPP_INCLUDE_DIRS} 38 | ) 39 | 40 | ############# 41 | ## Install ## 42 | ############# 43 | install(DIRECTORY include/${PROJECT_NAME}/ 44 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 45 | ) 46 | 47 | ############# 48 | ## Testing ## 49 | ############# 50 | if(CATKIN_ENABLE_TESTING) 51 | find_package(catkin REQUIRED COMPONENTS 52 | roscpp 53 | tf2_eigen 54 | ) 55 | find_package(rostest REQUIRED) 56 | catkin_add_gtest(utest test/utest.cpp) 57 | target_link_libraries(utest ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 58 | endif() 59 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright 2020 Southwest Research Institute 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # message_serialization 2 | ![](https://github.com/swri-robotics/message_serialization/workflows/CI/badge.svg) 3 | [![license - Apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0) 4 | 5 | A header-only utility for serializing C++ structures (specifically ROS messages) into YAML-formatted and binary-formatted files that can be loaded to/from disk 6 | 7 | ## Usage 8 | 9 | ```c++ 10 | // Serialization headers 11 | #include 12 | #include 13 | 14 | // Datatype-specific serialization header 15 | #include 16 | 17 | int main(int argc, char** argv) 18 | { 19 | // Create a structure to serialize 20 | geometry_msgs::PoseStamped ps; 21 | ps.header.frame = "world"; 22 | ps.header.stamp = ros::Time::now(); 23 | ... 24 | 25 | const std::string filename = "/path/to/save/dir/file.yaml"; 26 | 27 | // YAML Serialization 28 | // Serialize the message 29 | if(!message_serialization::serialize(filename, ps)) 30 | return -1; 31 | 32 | // De-serialize the message 33 | geometry_msgs::Pose new_ps; 34 | if(!message_serialization::deserialize(filename, new_ps)) 35 | return -1; 36 | 37 | // Binary serialization (ROS messages only) 38 | // Serialize the message 39 | if(!message_serialization::serializeToBinary(filename, ps)) 40 | return -1; 41 | 42 | // De-serialize the message 43 | if(!message_serialization::deserializeFromBinary(filename, new_ps)) 44 | return -1; 45 | 46 | return 0; 47 | } 48 | ``` 49 | 50 | ## Customization 51 | 52 | Any custom C++ structure can be serialized to YAML with this library, provided that a specific template structure for the custom datatype be specialized in the YAML namespace: 53 | 54 | ```c++ 55 | struct CustomStruct; 56 | 57 | namespace YAML 58 | { 59 | template<> 60 | struct convert 61 | { 62 | static Node encode(const CustomStruct& rhs); 63 | static bool decode(const Node& node, CustomStruct& rhs); 64 | }; 65 | } 66 | ``` 67 | 68 | See the implementations in the `include` directory for examples on how to implement this structure for a custom data type. 69 | -------------------------------------------------------------------------------- /include/message_serialization/binary_serialization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_BINARY_SERIALIZATION_H 17 | #define MESSAGE_SERIALIZATION_BINARY_SERIALIZATION_H 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace message_serialization 24 | { 25 | /** 26 | * @brief Serializes a ROS message to a binary file 27 | * @param file 28 | * @param message ROS message to serialize 29 | * @throws on failure to open or write to a file stream 30 | */ 31 | template 32 | inline void serializeToBinary(const T& message, const std::string& file) 33 | { 34 | uint32_t serial_size = ros::serialization::serializationLength(message); 35 | boost::shared_array buffer(new uint8_t[serial_size]); 36 | ros::serialization::OStream stream(buffer.get(), serial_size); 37 | ros::serialization::serialize(stream, message); 38 | 39 | std::ofstream ofs(file, std::ios::out|std::ios::binary); 40 | if(ofs) 41 | { 42 | ofs.write((char*) buffer.get(), serial_size); 43 | if(!ofs.good()) 44 | throw std::runtime_error("Failed to write to binary file stream at '" + file + "'"); 45 | } 46 | else 47 | { 48 | throw std::runtime_error("Failed to open binary file stream at '" + file + "'"); 49 | } 50 | } 51 | 52 | /** 53 | * @brief Serializes a ROS message to a binary file 54 | * @param file 55 | * @param message ROS message to serialize 56 | * @return 57 | */ 58 | template 59 | inline bool serializeToBinary(const std::string& file, const T& message) noexcept 60 | { 61 | try 62 | { 63 | serializeToBinary(message, file); 64 | } 65 | catch (const std::exception& ex) 66 | { 67 | ROS_ERROR_STREAM("Serialization error: " << ex.what()); 68 | return false; 69 | } 70 | 71 | return true; 72 | } 73 | 74 | /** 75 | * @brief De-serializes a binary file into a ROS message 76 | * @param file 77 | * @param message (output) ROS message 78 | * @return 79 | * @throws on failure to open or read a file stream 80 | */ 81 | template 82 | inline T deserializeFromBinary(const std::string& file) 83 | { 84 | std::ifstream ifs(file, std::ios::in | std::ios::binary); 85 | if (!ifs) 86 | throw std::runtime_error("Failed to open binary file stream at '" + file + "'"); 87 | 88 | ifs.seekg(0, std::ios::end); 89 | std::streampos end = ifs.tellg(); 90 | ifs.seekg(0, std::ios::beg); 91 | std::streampos begin = ifs.tellg(); 92 | uint32_t file_size = end - begin; 93 | 94 | boost::shared_array ibuffer(new uint8_t[file_size]); 95 | ifs.read((char*)ibuffer.get(), file_size); 96 | 97 | T message; 98 | ros::serialization::IStream istream(ibuffer.get(), file_size); 99 | ros::serialization::deserialize(istream, message); 100 | 101 | ifs.close(); 102 | 103 | return message; 104 | } 105 | 106 | /** 107 | * @brief De-serializes a binary file into a ROS message 108 | * @param file 109 | * @param message (output) ROS message 110 | * @return 111 | */ 112 | template 113 | inline bool deserializeFromBinary(const std::string& file, T& message) noexcept 114 | { 115 | try 116 | { 117 | message = deserializeFromBinary(file); 118 | } 119 | catch (const std::exception &ex) 120 | { 121 | ROS_ERROR_STREAM("Deserialization error: '" << ex.what() << "'"); 122 | return false; 123 | } 124 | 125 | return true; 126 | } 127 | 128 | /** 129 | * @brief Serializes a ROS message into a shared_array 130 | * @param buffer (output) Data array buffer 131 | * @param message ROS message to serialize 132 | * @return number of bytes in the buffer 133 | */ 134 | template 135 | inline uint32_t serializeToBuffer(boost::shared_array& buffer, const T& message) 136 | { 137 | uint32_t serial_size = ros::serialization::serializationLength(message); 138 | buffer.reset(new uint8_t[serial_size]); 139 | ros::serialization::OStream stream(buffer.get(), serial_size); 140 | ros::serialization::serialize(stream, message); 141 | return serial_size; 142 | } 143 | 144 | /** 145 | * @brief De-serializes an array of known length into a ROS message 146 | * @param buffer Data array buffer 147 | * @param size Buffer size 148 | * @param message (output) ROS message 149 | * @return success 150 | * @throws on failure to deserialize the buffer 151 | */ 152 | template 153 | inline T deserializeFromBuffer(uint8_t* const buffer, const uint32_t size) 154 | { 155 | T message; 156 | ros::serialization::IStream istream(buffer, size); 157 | ros::serialization::deserialize(istream, message); 158 | return message; 159 | } 160 | 161 | /** 162 | * @brief De-serializes an array of known length into a ROS message 163 | * @param buffer Data array buffer 164 | * @param size Buffer size 165 | * @param message (output) ROS message 166 | * @return success 167 | */ 168 | template 169 | inline bool deserializeFromBuffer(uint8_t* const buffer, const uint32_t size, T& message) noexcept 170 | { 171 | try 172 | { 173 | message = deserializeFromBinary(buffer, size); 174 | } 175 | catch (const std::exception& ex) 176 | { 177 | ROS_ERROR_STREAM("Deserialization error: '" << ex.what() << "'"); 178 | return false; 179 | } 180 | return true; 181 | } 182 | } // namespace message_serialization 183 | 184 | #endif // MESSAGE_SERIALIZATION_BINARY_SERIALIZATION_H 185 | -------------------------------------------------------------------------------- /include/message_serialization/eigen_yaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_EIGEN_YAML_H 17 | #define MESSAGE_SERIALIZATION_EIGEN_YAML_H 18 | 19 | #include 20 | #include 21 | 22 | namespace YAML 23 | { 24 | template<> 25 | struct convert 26 | { 27 | static Node encode(const Eigen::Isometry3d &rhs) 28 | { 29 | geometry_msgs::Pose msg; 30 | tf::poseEigenToMsg(rhs, msg); 31 | Node node = Node(msg); 32 | return node; 33 | } 34 | 35 | static bool decode(const Node &node, Eigen::Isometry3d &rhs) 36 | { 37 | geometry_msgs::Pose msg; 38 | msg = node.as(); 39 | tf::poseMsgToEigen(msg, rhs); 40 | return true; 41 | } 42 | }; 43 | 44 | template<> 45 | struct convert 46 | { 47 | static Node encode(const Eigen::Affine3d& rhs) 48 | { 49 | geometry_msgs::Pose msg; 50 | tf::poseEigenToMsg(rhs, msg); 51 | Node node = Node(msg); 52 | return node; 53 | } 54 | 55 | static bool decode(const Node& node, Eigen::Affine3d& rhs) 56 | { 57 | geometry_msgs::Pose msg; 58 | msg = node.as(); 59 | tf::poseMsgToEigen(msg, rhs); 60 | return true; 61 | } 62 | }; 63 | 64 | template<> 65 | struct convert 66 | { 67 | static Node encode(const Eigen::Vector3d& rhs) 68 | { 69 | geometry_msgs::Point msg; 70 | tf::pointEigenToMsg(rhs, msg); 71 | Node node = Node(msg); 72 | return node; 73 | } 74 | 75 | static bool decode(const Node& node, Eigen::Vector3d& rhs) 76 | { 77 | geometry_msgs::Point msg; 78 | msg = node.as(); 79 | tf::pointMsgToEigen(msg, rhs); 80 | return true; 81 | } 82 | }; 83 | 84 | } 85 | 86 | #endif // MESSAGE_SERIALIZATION_EIGEN_YAML_H 87 | -------------------------------------------------------------------------------- /include/message_serialization/geometry_msgs_yaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML 17 | #define MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace YAML 25 | { 26 | 27 | template<> 28 | struct convert 29 | { 30 | static Node encode(const geometry_msgs::Vector3& rhs) 31 | { 32 | Node node; 33 | node["x"] = rhs.x; 34 | node["y"] = rhs.y; 35 | node["z"] = rhs.z; 36 | 37 | return node; 38 | } 39 | 40 | static bool decode(const Node& node, geometry_msgs::Vector3& rhs) 41 | { 42 | if (node.size() != 3) return false; 43 | 44 | rhs.x = node["x"].as(); 45 | rhs.y = node["y"].as(); 46 | rhs.z = node["z"].as(); 47 | return true; 48 | } 49 | }; 50 | 51 | template<> 52 | struct convert 53 | { 54 | static Node encode(const geometry_msgs::Point& rhs) 55 | { 56 | Node node; 57 | node["x"] = rhs.x; 58 | node["y"] = rhs.y; 59 | node["z"] = rhs.z; 60 | return node; 61 | } 62 | 63 | static bool decode(const Node& node, geometry_msgs::Point& rhs) 64 | { 65 | if (node.size() != 3) return false; 66 | 67 | rhs.x = node["x"].as(); 68 | rhs.y = node["y"].as(); 69 | rhs.z = node["z"].as(); 70 | 71 | return true; 72 | } 73 | }; 74 | 75 | 76 | template<> 77 | struct convert 78 | { 79 | static Node encode(const geometry_msgs::Quaternion& rhs) 80 | { 81 | Node node; 82 | node["x"] = rhs.x; 83 | node["y"] = rhs.y; 84 | node["z"] = rhs.z; 85 | node["w"] = rhs.w; 86 | 87 | return node; 88 | } 89 | 90 | static bool decode(const Node& node, geometry_msgs::Quaternion& rhs) 91 | { 92 | if (node.size() != 4) return false; 93 | 94 | rhs.x = node["x"].as(); 95 | rhs.y = node["y"].as(); 96 | rhs.z = node["z"].as(); 97 | rhs.w = node["w"].as(); 98 | 99 | return true; 100 | } 101 | }; 102 | 103 | template<> 104 | struct convert 105 | { 106 | static Node encode(const geometry_msgs::Pose& rhs) 107 | { 108 | Node node; 109 | node["position"] = rhs.position; 110 | node["orientation"] = rhs.orientation; 111 | 112 | return node; 113 | } 114 | 115 | static bool decode(const Node& node, geometry_msgs::Pose& rhs) 116 | { 117 | if (node.size() != 2) return false; 118 | 119 | rhs.position = node["position"].as(); 120 | rhs.orientation = node["orientation"].as(); 121 | return true; 122 | } 123 | }; 124 | 125 | template<> 126 | struct convert 127 | { 128 | static Node encode(const geometry_msgs::PoseStamped& rhs) 129 | { 130 | Node node; 131 | node["header"] = rhs.header; 132 | node["pose"] = rhs.pose; 133 | 134 | return node; 135 | } 136 | 137 | static bool decode(const Node& node, geometry_msgs::PoseStamped& rhs) 138 | { 139 | if (node.size() != 2) return false; 140 | 141 | rhs.header = node["header"].as(); 142 | rhs.pose = node["pose"].as(); 143 | return true; 144 | } 145 | }; 146 | 147 | template<> 148 | struct convert 149 | { 150 | static Node encode(const geometry_msgs::PoseArray& rhs) 151 | { 152 | Node node; 153 | node["header"] = rhs.header; 154 | node["poses"] = rhs.poses; 155 | 156 | return node; 157 | } 158 | 159 | static bool decode(const Node& node, geometry_msgs::PoseArray& rhs) 160 | { 161 | if (node.size() != 2) return false; 162 | 163 | rhs.header = node["header"].as(); 164 | rhs.poses = node["poses"].as >(); 165 | return true; 166 | } 167 | }; 168 | 169 | template<> 170 | struct convert 171 | { 172 | static Node encode(const geometry_msgs::Transform& rhs) 173 | { 174 | Node node; 175 | node["rotation"] = rhs.rotation; 176 | node["translation"] = rhs.translation; 177 | 178 | return node; 179 | } 180 | 181 | static bool decode(const Node& node, geometry_msgs::Transform& rhs) 182 | { 183 | if (node.size() != 2) return false; 184 | 185 | rhs.rotation = node["rotation"].as(); 186 | rhs.translation = node["translation"].as(); 187 | 188 | return true; 189 | } 190 | }; 191 | 192 | template<> 193 | struct convert 194 | { 195 | static Node encode(const geometry_msgs::TransformStamped& rhs) 196 | { 197 | Node node; 198 | node["header"] = rhs.header; 199 | node["child_frame_id"] = rhs.child_frame_id; 200 | node["transform"] = rhs.transform; 201 | 202 | return node; 203 | } 204 | 205 | static bool decode(const Node& node, geometry_msgs::TransformStamped& rhs) 206 | { 207 | if (node.size() != 3) return false; 208 | 209 | rhs.header = node["header"].as(); 210 | rhs.child_frame_id = node["child_frame_id"].as(); 211 | rhs.transform = node["transform"].as(); 212 | return true; 213 | } 214 | }; 215 | 216 | } 217 | 218 | #endif // MESSAGE_SERIALIZATION_GEOMETRY_MSGS_YAML 219 | -------------------------------------------------------------------------------- /include/message_serialization/sensor_msgs_yaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_SENSOR_MSGS_YAML 17 | #define MESSAGE_SERIALIZATION_SENSOR_MSGS_YAML 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace YAML 24 | { 25 | 26 | template<> 27 | struct convert 28 | { 29 | static Node encode(const sensor_msgs::RegionOfInterest& rhs) 30 | { 31 | Node node; 32 | 33 | node["x_offset"] = rhs.x_offset; 34 | node["y_offset"] = rhs.y_offset; 35 | node["height"] = rhs.height; 36 | node["width"] = rhs.width; 37 | node["do_rectify"] = rhs.do_rectify; 38 | 39 | return node; 40 | } 41 | 42 | static bool decode(const Node& node, sensor_msgs::RegionOfInterest& rhs) 43 | { 44 | if (node.size() != 5) return false; 45 | 46 | rhs.x_offset = node["x_offset"].as(); 47 | rhs.y_offset = node["y_offset"].as(); 48 | rhs.height = node["height"].as(); 49 | rhs.width = node["width"].as(); 50 | rhs.do_rectify = node["do_rectify"].as() ; 51 | 52 | return true; 53 | } 54 | }; 55 | 56 | template<> 57 | struct convert 58 | { 59 | static Node encode(const sensor_msgs::CameraInfo& rhs) 60 | { 61 | Node node; 62 | 63 | node["header"] = rhs.header; 64 | node["height"] = rhs.height; 65 | node["width"] = rhs.width; 66 | node["distortion_model"] = rhs.distortion_model; 67 | 68 | std::vector K_vec, R_vec, P_vec; 69 | 70 | std::copy_n(rhs.K.begin(), rhs.K.size(), std::back_inserter(K_vec)); 71 | std::copy_n(rhs.R.begin(), rhs.R.size(), std::back_inserter(R_vec)); 72 | std::copy_n(rhs.P.begin(), rhs.P.size(), std::back_inserter(P_vec)); 73 | 74 | node["D"] = rhs.D; 75 | node["K"] = K_vec; 76 | node["R"] = R_vec; 77 | node["P"] = P_vec; 78 | node["binning_x"] = rhs.binning_x; 79 | node["binning_y"] = rhs.binning_y; 80 | node["roi"] = rhs.roi; 81 | 82 | return node; 83 | } 84 | 85 | static bool decode(const Node& node, sensor_msgs::CameraInfo& rhs) 86 | { 87 | if (node.size() != 11) return false; 88 | 89 | rhs.header = node["header"].as(); 90 | rhs.height = node["height"].as(); 91 | rhs.width = node["width"].as(); 92 | rhs.distortion_model = node["distortion_model"].as(); 93 | rhs.D = node["D"].as(); 94 | 95 | std::vector K_vec, R_vec, P_vec; 96 | K_vec = node["K"].as(); 97 | R_vec = node["R"].as(); 98 | P_vec = node["P"].as(); 99 | 100 | std::copy_n(K_vec.begin(), K_vec.size(), rhs.K.begin()); 101 | std::copy_n(R_vec.begin(), R_vec.size(), rhs.R.begin()); 102 | std::copy_n(P_vec.begin(), P_vec.size(), rhs.P.begin()); 103 | 104 | rhs.binning_x = node["binning_x"].as(); 105 | rhs.binning_y = node["binning_y"].as(); 106 | rhs.roi = node["roi"].as(); 107 | 108 | return true; 109 | } 110 | }; 111 | 112 | template<> 113 | struct convert 114 | { 115 | static Node encode(const sensor_msgs::JointState& rhs) 116 | { 117 | Node node; 118 | 119 | node["header"] = rhs.header; 120 | node["name"] = rhs.name; 121 | node["position"] = rhs.position; 122 | node["velocity"] = rhs.velocity; 123 | node["effort"] = rhs.effort; 124 | 125 | return node; 126 | } 127 | 128 | static bool decode(const Node& node, sensor_msgs::JointState& rhs) 129 | { 130 | if (node.size() != 5) return false; 131 | 132 | rhs.header = node["header"].as(); 133 | rhs.name = node["name"].as(); 134 | rhs.position = node["position"].as(); 135 | rhs.velocity = node["velocity"].as(); 136 | rhs.effort = node["effort"].as() ; 137 | 138 | return true; 139 | } 140 | }; 141 | 142 | } // namespace YAML 143 | 144 | #endif // MESSAGE_SERIALIZATION_SENSOR_MSGS_YAML 145 | -------------------------------------------------------------------------------- /include/message_serialization/serialize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_SERIALIZE_H 17 | #define MESSAGE_SERIALIZATION_SERIALIZE_H 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace message_serialization 24 | { 25 | /** 26 | * @brief Serializes an input object to a YAML-formatted file 27 | * @param val 28 | * @param file 29 | * @throws exception on failure to open or write to a file stream 30 | */ 31 | template 32 | inline void serialize(const T& val, const std::string& file) 33 | { 34 | std::ofstream ofh(file); 35 | if (!ofh) 36 | throw std::runtime_error("Failed to open output file stream at '" + file + "'"); 37 | 38 | YAML::Node n = YAML::Node(val); 39 | ofh << n; 40 | } 41 | 42 | /** 43 | * @brief Serializes an input object to a YAML-formatted file 44 | * @param file 45 | * @param val 46 | * @return true on success, false otherwise 47 | */ 48 | template 49 | inline bool serialize(const std::string& file, const T& val) noexcept 50 | { 51 | try 52 | { 53 | serialize(val, file); 54 | } 55 | catch (const std::exception& ex) 56 | { 57 | ROS_ERROR_STREAM(ex.what()); 58 | return false; 59 | } 60 | return true; 61 | } 62 | 63 | /** 64 | * @brief Deserializes a YAML-formatted file into a specific object type 65 | * @param file 66 | * @return 67 | * @throws exception when unable to load the file or convert it to the specified type 68 | */ 69 | template 70 | inline T deserialize(const std::string &file) 71 | { 72 | YAML::Node node; 73 | node = YAML::LoadFile(file); 74 | return node.as(); 75 | } 76 | 77 | /** 78 | * @brief Deserializes a YAML-formatted file into a specific object type 79 | * @param file 80 | * @param val 81 | * @return true on success, false otherwise 82 | */ 83 | template 84 | inline bool deserialize(const std::string &file, T& val) noexcept 85 | { 86 | try 87 | { 88 | val = deserialize(file); 89 | } 90 | catch (const std::exception& ex) 91 | { 92 | ROS_ERROR_STREAM("Deserialization error: " << ex.what()); 93 | return false; 94 | } 95 | return true; 96 | } 97 | 98 | } // namespace message_serialization 99 | 100 | #endif // MESSAGE_SERIALIZATION_SERIALIZE_H 101 | -------------------------------------------------------------------------------- /include/message_serialization/shape_msgs_yaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_SHAPE_MSGS_YAML 17 | #define MESSAGE_SERIALIZATION_SHAPE_MSGS_YAML 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace YAML 24 | { 25 | template<> 26 | struct convert 27 | { 28 | static Node encode(const shape_msgs::MeshTriangle& rhs) 29 | { 30 | Node node; 31 | std::vector tv; 32 | tv.push_back(rhs.vertex_indices[0]); 33 | tv.push_back(rhs.vertex_indices[1]); 34 | tv.push_back(rhs.vertex_indices[2]); 35 | node["vertex_indices"] = tv; 36 | return node; 37 | } 38 | 39 | static bool decode(const Node& node, shape_msgs::MeshTriangle& rhs) 40 | { 41 | std::vector tv; 42 | tv = node["vertex_indices"].as(); 43 | rhs.vertex_indices[0] = tv[0]; 44 | rhs.vertex_indices[1] = tv[1]; 45 | rhs.vertex_indices[2] = tv[2]; 46 | return true; 47 | } 48 | }; 49 | 50 | template<> 51 | struct convert 52 | { 53 | static Node encode(const shape_msgs::Mesh& rhs) 54 | { 55 | Node node; 56 | node["triangles"] = rhs.triangles; 57 | node["vertices"] = rhs.vertices; 58 | 59 | return node; 60 | } 61 | 62 | static bool decode(const Node& node, shape_msgs::Mesh& rhs) 63 | { 64 | if (node.size() != 2) return false; 65 | 66 | rhs.triangles = node["triangles"].as(); 67 | rhs.vertices = node["vertices"].as(); 68 | 69 | return true; 70 | } 71 | }; 72 | 73 | } 74 | 75 | #endif // MESSAGE_SERIALIZATION_SHAPE_MSGS_YAML 76 | -------------------------------------------------------------------------------- /include/message_serialization/std_msgs_yaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_STD_MSGS_YAML 17 | #define MESSAGE_SERIALIZATION_STD_MSGS_YAML 18 | 19 | #include 20 | #include 21 | 22 | namespace YAML 23 | { 24 | 25 | template<> 26 | struct convert 27 | { 28 | static Node encode(const ros::Time& rhs) 29 | { 30 | Node node; 31 | node["sec"] = rhs.sec; 32 | node["nsec"] = rhs.nsec; 33 | return node; 34 | } 35 | 36 | static bool decode(const Node& node, ros::Time& rhs) 37 | { 38 | if (node.size() != 2) return false; 39 | 40 | rhs.sec = node["sec"].as(); 41 | rhs.nsec = node["nsec"].as(); 42 | 43 | return true; 44 | } 45 | }; 46 | 47 | template<> 48 | struct convert 49 | { 50 | static Node encode(const std_msgs::Header& rhs) 51 | { 52 | Node node; 53 | node["seq"] = rhs.seq; 54 | node["stamp"] = rhs.stamp; 55 | node["frame_id"] = rhs.frame_id; 56 | return node; 57 | } 58 | 59 | static bool decode(const Node& node, std_msgs::Header& rhs) 60 | { 61 | if (node.size() != 3) return false; 62 | 63 | rhs.seq = node["seq"].as(); 64 | rhs.stamp = node["stamp"].as(); 65 | rhs.frame_id = node["frame_id"].as(); 66 | 67 | return true; 68 | } 69 | }; 70 | 71 | // TODO: Duration 72 | 73 | } 74 | 75 | #endif // MESSAGE_SERIALIZATION_STD_MSGS_YAML 76 | -------------------------------------------------------------------------------- /include/message_serialization/trajectory_msgs_yaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Southwest Research Institute 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML 17 | #define MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML 18 | 19 | #include 20 | #include 21 | 22 | namespace YAML 23 | { 24 | 25 | template<> 26 | struct convert 27 | { 28 | static Node encode(const trajectory_msgs::JointTrajectoryPoint& rhs) 29 | { 30 | Node node; 31 | node["positions"] = rhs.positions; 32 | node["velocities"] = rhs.velocities; 33 | node["accelerations"] = rhs.accelerations; 34 | node["effort"] = rhs.effort; 35 | node["time_from_start"] = rhs.time_from_start.toSec(); 36 | 37 | return node; 38 | } 39 | 40 | static bool decode(const Node& node, trajectory_msgs::JointTrajectoryPoint& rhs) 41 | { 42 | if (node.size() != 5) return false; 43 | 44 | rhs.positions = node["positions"].as >(); 45 | rhs.velocities = node["velocities"].as >(); 46 | rhs.accelerations = node["accelerations"].as >(); 47 | rhs.effort = node["effort"].as >(); 48 | rhs.time_from_start = ros::Duration(node["time_from_start"].as()); 49 | 50 | return true; 51 | } 52 | }; 53 | 54 | template<> 55 | struct convert 56 | { 57 | static Node encode(const trajectory_msgs::JointTrajectory& rhs) 58 | { 59 | Node node; 60 | node["header"] = rhs.header; 61 | node["joint_names"] = rhs.joint_names; 62 | node["points"] = rhs.points; 63 | return node; 64 | } 65 | 66 | static bool decode(const Node& node, trajectory_msgs::JointTrajectory& rhs) 67 | { 68 | if (node.size() != 3) return false; 69 | 70 | rhs.header = node["header"].as(); 71 | rhs.joint_names = node["joint_names"].as >(); 72 | rhs.points = node["points"].as >(); 73 | 74 | return true; 75 | } 76 | }; 77 | 78 | } 79 | 80 | #endif // MESSAGE_SERIALIZATION_TRAJECTORY_MSGS_YAML 81 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | message_serialization 4 | 0.0.0 5 | The message_serialization package 6 | 7 | Jon Meyer 8 | Michael Ripperger 9 | Michael Ripperger 10 | 11 | Apache 2.0 12 | 13 | catkin 14 | eigen_conversions 15 | geometry_msgs 16 | sensor_msgs 17 | std_msgs 18 | roscpp_serialization 19 | trajectory_msgs 20 | yaml-cpp 21 | rospy_message_converter 22 | rostest 23 | roscpp 24 | tf2_eigen 25 | 26 | -------------------------------------------------------------------------------- /scripts/message_converter.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env/python 2 | # This Python file uses the following encoding: utf-8 3 | 4 | """ 5 | Copyright 2020 Southwest Research Institute 6 | 7 | Licensed under the Apache License, Version 2.0 (the "License"); 8 | you may not use this file except in compliance with the License. 9 | You may obtain a copy of the License at 10 | 11 | http://www.apache.org/licenses/LICENSE-2.0 12 | 13 | Unless required by applicable law or agreed to in writing, software 14 | distributed under the License is distributed on an "AS IS" BASIS, 15 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | see the License for the specific language governing permissions and 17 | limitations under the License. 18 | """ 19 | 20 | from rospy_message_converter import json_message_converter as jmc 21 | import json 22 | import rospy 23 | import sys 24 | from StringIO import StringIO 25 | import argparse 26 | 27 | class Converter: 28 | def __init__(self, module_name, msg_class): 29 | self.module = __import__(module_name) 30 | self.msg_class = getattr(self.module.msg, msg_class) 31 | 32 | def binaryFileToROS(self, filename): 33 | msg = self.msg_class() 34 | with open(filename, 'r') as file: 35 | msg = msg.deserialize(file.read()) 36 | return msg 37 | 38 | def rosToBinaryFile(self, msg, filename): 39 | ser = StringIO() 40 | msg.serialize(ser) 41 | with open(filename, 'w') as file: 42 | file.write(ser.getvalue()) 43 | 44 | def jsonFileToROS(self, filename): 45 | json_str = '' 46 | with open(filename, 'r') as file: 47 | data = json.load(file) 48 | json_str = json.dumps(data) 49 | return jmc.convert_json_to_ros_message(self.msg_class()._type, json_str) 50 | 51 | def rosToJsonFile(self, msg, filename): 52 | json_str = jmc.convert_ros_message_to_json(msg) 53 | json_obj = json.loads(json_str) 54 | with open(filename, 'w') as file: 55 | file = json.dump(json_obj, file, indent=2) 56 | 57 | 58 | if __name__ == "__main__": 59 | parser = argparse.ArgumentParser(description='Converts binary ROS message files to JSON files and vice versa') 60 | parser.add_argument('msg_module', help='The module from which to load the ROS message class (i.e. std_msgs.msg)') 61 | parser.add_argument('msg_class', help='The ROS message class name (i.e. Header)') 62 | parser.add_argument('bin_file', help="The binary ROS message file to read/write from") 63 | parser.add_argument('json_file', help="The JSON formatted ROS message file to read/write from") 64 | parser.add_argument('-b2j', '--binary-to-json', action='store_true', help="Convert binary ROS file to JSON formatted ROS file") 65 | parser.add_argument('-j2b', '--json-to-binary', action='store_true', help="Convert JSON formatted ROS File to binary ROS file") 66 | 67 | args = parser.parse_args() 68 | 69 | try: 70 | if (args.json_to_binary and args.binary_to_json) or (not args.json_to_binary and not args.binary_to_json): 71 | raise Exception("Choose a single direction of conversion!") 72 | 73 | conv = Converter(args.msg_module, args.msg_class) 74 | if args.json_to_binary: 75 | msg = conv.jsonFileToROS(args.json_file) 76 | conv.rosToBinaryFile(msg, args.bin_file) 77 | else: 78 | msg = conv.binaryFileToROS(args.bin_file) 79 | conv.rosToJsonFile(msg, args.json_file) 80 | 81 | except Exception as ex: 82 | print("Failure: " + str(ex)) 83 | -------------------------------------------------------------------------------- /test/geometry_msgs_test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "std_msgs_test.h" 4 | #include 5 | #include 6 | 7 | template <> 8 | geometry_msgs::Quaternion create() 9 | { 10 | return tf2::toMsg(Eigen::Quaterniond::UnitRandom()); 11 | } 12 | 13 | template <> 14 | bool equals(const geometry_msgs::Quaternion& lhs, const geometry_msgs::Quaternion& rhs) 15 | { 16 | Eigen::Quaterniond q_lhs, q_rhs; 17 | tf2::fromMsg(lhs, q_lhs); 18 | tf2::fromMsg(rhs, q_rhs); 19 | return q_lhs.isApprox(q_rhs); 20 | } 21 | 22 | template <> 23 | geometry_msgs::Point create() 24 | { 25 | const Eigen::Vector3d v = Eigen::Vector3d::Random(); 26 | return tf2::toMsg(v); 27 | } 28 | 29 | template <> 30 | bool equals(const geometry_msgs::Point& lhs, const geometry_msgs::Point& rhs) 31 | { 32 | Eigen::Vector3d v_lhs, v_rhs; 33 | tf2::fromMsg(lhs, v_lhs); 34 | tf2::fromMsg(rhs, v_rhs); 35 | return v_lhs.isApprox(v_rhs); 36 | } 37 | 38 | template <> 39 | geometry_msgs::Vector3 create() 40 | { 41 | geometry_msgs::Vector3 v; 42 | return tf2::toMsg(Eigen::Vector3d::Random(), v); 43 | } 44 | 45 | template <> 46 | bool equals(const geometry_msgs::Vector3& lhs, const geometry_msgs::Vector3& rhs) 47 | { 48 | Eigen::Vector3d v_lhs, v_rhs; 49 | tf2::fromMsg(lhs, v_lhs); 50 | tf2::fromMsg(rhs, v_rhs); 51 | return v_lhs.isApprox(v_rhs); 52 | } 53 | 54 | template <> 55 | geometry_msgs::Pose create() 56 | { 57 | geometry_msgs::Pose p; 58 | p.position = create(); 59 | p.orientation = create(); 60 | return p; 61 | } 62 | 63 | template <> 64 | bool equals(const geometry_msgs::Pose& lhs, const geometry_msgs::Pose& rhs) 65 | { 66 | bool eq = true; 67 | eq &= equals(lhs.position, rhs.position); 68 | eq &= equals(lhs.orientation, rhs.orientation); 69 | return eq; 70 | } 71 | 72 | template <> 73 | geometry_msgs::PoseStamped create() 74 | { 75 | geometry_msgs::PoseStamped p; 76 | p.pose = create(); 77 | p.header = create(); 78 | return p; 79 | } 80 | 81 | template <> 82 | bool equals(const geometry_msgs::PoseStamped& lhs, const geometry_msgs::PoseStamped& rhs) 83 | { 84 | bool eq = true; 85 | eq &= equals(lhs.pose, rhs.pose); 86 | eq &= equals(lhs.header, rhs.header); 87 | return eq; 88 | } 89 | 90 | template <> 91 | geometry_msgs::PoseArray create() 92 | { 93 | geometry_msgs::PoseArray arr; 94 | arr.poses.resize(5); 95 | std::generate(arr.poses.begin(), arr.poses.end(), []()->geometry_msgs::Pose { return create(); }); 96 | arr.header = create(); 97 | return arr; 98 | } 99 | 100 | template <> 101 | bool equals(const geometry_msgs::PoseArray& lhs, const geometry_msgs::PoseArray& rhs) 102 | { 103 | bool eq = true; 104 | eq &= equals(lhs.poses, rhs.poses); 105 | eq &= equals(lhs.header, rhs.header); 106 | return eq; 107 | } 108 | 109 | template <> 110 | geometry_msgs::Transform create() 111 | { 112 | geometry_msgs::Transform p; 113 | p.translation = create(); 114 | p.rotation = create(); 115 | return p; 116 | } 117 | 118 | template <> 119 | bool equals(const geometry_msgs::Transform& lhs, const geometry_msgs::Transform& rhs) 120 | { 121 | bool eq = true; 122 | eq &= equals(lhs.translation, rhs.translation); 123 | eq &= equals(lhs.rotation, rhs.rotation); 124 | return eq; 125 | } 126 | 127 | template <> 128 | geometry_msgs::TransformStamped create() 129 | { 130 | geometry_msgs::TransformStamped p; 131 | p.transform = create(); 132 | p.header = create(); 133 | return p; 134 | } 135 | 136 | template <> 137 | bool equals(const geometry_msgs::TransformStamped& lhs, const geometry_msgs::TransformStamped& rhs) 138 | { 139 | bool eq = true; 140 | eq &= equals(lhs.transform, rhs.transform); 141 | eq &= equals(lhs.header, rhs.header); 142 | return eq; 143 | } 144 | -------------------------------------------------------------------------------- /test/sensor_msgs_test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "std_msgs_test.h" 4 | #include 5 | 6 | template <> 7 | sensor_msgs::JointState create() 8 | { 9 | const std::size_t n = 10; 10 | sensor_msgs::JointState js; 11 | js.header = create(); 12 | js.name.resize(n); 13 | std::fill(js.name.begin(), js.name.end(), "test"); 14 | js.position = createRandomVector(n); 15 | js.velocity = createRandomVector(n); 16 | js.effort = createRandomVector(n); 17 | 18 | return js; 19 | } 20 | 21 | template <> 22 | bool equals(const sensor_msgs::JointState& lhs, const sensor_msgs::JointState& rhs) 23 | { 24 | bool eq = true; 25 | eq &= equals(lhs.header, rhs.header); 26 | eq &= lhs.name == rhs.name; 27 | eq &= isApprox(lhs.position.data(), rhs.position.data(), lhs.position.size()); 28 | eq &= isApprox(lhs.velocity.data(), rhs.velocity.data(), lhs.velocity.size()); 29 | eq &= isApprox(lhs.effort.data(), rhs.effort.data(), lhs.velocity.size()); 30 | return eq; 31 | } 32 | 33 | template <> 34 | sensor_msgs::RegionOfInterest create() 35 | { 36 | sensor_msgs::RegionOfInterest roi; 37 | roi.width = 511; 38 | roi.height = 1567; 39 | roi.x_offset = 12; 40 | roi.y_offset = 456; 41 | roi.do_rectify = true; 42 | return roi; 43 | } 44 | 45 | template <> 46 | bool equals(const sensor_msgs::RegionOfInterest& lhs, const sensor_msgs::RegionOfInterest& rhs) 47 | { 48 | return lhs == rhs; 49 | } 50 | 51 | template <> 52 | sensor_msgs::CameraInfo create() 53 | { 54 | sensor_msgs::CameraInfo info; 55 | info.header = create(); 56 | randomize(info.D.data(), info.D.size()); 57 | randomize(info.K.data(), info.K.size()); 58 | randomize(info.P.data(), info.P.size()); 59 | randomize(info.R.data(), info.R.size()); 60 | info.roi = create(); 61 | info.width = 12345; 62 | info.height = 54321; 63 | info.binning_x = 50; 64 | info.binning_y = 75; 65 | info.distortion_model = "test"; 66 | 67 | return info; 68 | } 69 | 70 | template <> 71 | bool equals(const sensor_msgs::CameraInfo& lhs, const sensor_msgs::CameraInfo& rhs) 72 | { 73 | bool eq = true; 74 | eq &= equals(lhs.header, rhs.header); 75 | eq &= isApprox(lhs.D.data(), rhs.D.data(), lhs.D.size()); 76 | eq &= isApprox(lhs.K.data(), rhs.K.data(), lhs.K.size()); 77 | eq &= isApprox(lhs.P.data(), rhs.P.data(), lhs.P.size()); 78 | eq &= isApprox(lhs.R.data(), rhs.R.data(), lhs.R.size()); 79 | eq &= equals(lhs.roi, rhs.roi); 80 | eq &= lhs.width == rhs.width; 81 | eq &= lhs.height == rhs.height; 82 | eq &= lhs.binning_x == rhs.binning_x; 83 | eq &= lhs.binning_y == rhs.binning_y; 84 | eq &= lhs.distortion_model == rhs.distortion_model; 85 | 86 | return eq; 87 | } 88 | 89 | -------------------------------------------------------------------------------- /test/shape_msgs_test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "geometry_msgs_test.h" 4 | #include 5 | 6 | template <> 7 | shape_msgs::MeshTriangle create() 8 | { 9 | shape_msgs::MeshTriangle tri; 10 | tri.vertex_indices[0] = 4; 11 | tri.vertex_indices[1] = 3; 12 | tri.vertex_indices[2] = 5; 13 | return tri; 14 | } 15 | 16 | template <> 17 | bool equals(const shape_msgs::MeshTriangle& lhs, const shape_msgs::MeshTriangle& rhs) 18 | { 19 | return lhs == rhs; 20 | } 21 | 22 | template <> 23 | shape_msgs::Mesh create() 24 | { 25 | shape_msgs::Mesh mesh; 26 | const std::size_t n = 10; 27 | mesh.vertices.resize(n); 28 | std::generate(mesh.vertices.begin(), mesh.vertices.end(), 29 | []() -> geometry_msgs::Point { return create(); }); 30 | 31 | mesh.triangles.resize(n); 32 | std::generate(mesh.triangles.begin(), mesh.triangles.end(), 33 | []() -> shape_msgs::MeshTriangle { return create(); }); 34 | 35 | return mesh; 36 | } 37 | 38 | template <> 39 | bool equals(const shape_msgs::Mesh& lhs, const shape_msgs::Mesh& rhs) 40 | { 41 | bool eq = true; 42 | eq &= equals(lhs.vertices, rhs.vertices); 43 | eq &= equals(lhs.triangles, rhs.triangles); 44 | return eq; 45 | } 46 | -------------------------------------------------------------------------------- /test/std_msgs_test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "utilities.h" 4 | #include 5 | 6 | template <> 7 | ros::Time create() 8 | { 9 | return ros::Time(5.1); 10 | } 11 | 12 | template <> 13 | bool equals(const ros::Time& lhs, const ros::Time& rhs) 14 | { 15 | return lhs == rhs; 16 | } 17 | 18 | template <> 19 | std_msgs::Header create() 20 | { 21 | std_msgs::Header msg; 22 | msg.stamp = create(); 23 | msg.seq = 2; 24 | msg.frame_id = "test"; 25 | return msg; 26 | } 27 | 28 | template <> 29 | bool equals(const std_msgs::Header& lhs, const std_msgs::Header& rhs) 30 | { 31 | return lhs == rhs; 32 | } 33 | -------------------------------------------------------------------------------- /test/trajectory_msgs_test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "std_msgs_test.h" 4 | #include 5 | 6 | template <> 7 | trajectory_msgs::JointTrajectoryPoint create() 8 | { 9 | trajectory_msgs::JointTrajectoryPoint pt; 10 | 11 | std::size_t n = 10; 12 | pt.positions = createRandomVector(n); 13 | pt.velocities = createRandomVector(n); 14 | pt.accelerations = createRandomVector(n); 15 | pt.effort = createRandomVector(n); 16 | pt.time_from_start = ros::Duration(1.1); 17 | 18 | return pt; 19 | } 20 | 21 | template <> 22 | bool equals(const trajectory_msgs::JointTrajectoryPoint& lhs, const trajectory_msgs::JointTrajectoryPoint& rhs) 23 | { 24 | bool eq = true; 25 | eq &= isApprox(lhs.positions.data(), rhs.positions.data(), lhs.positions.size()); 26 | eq &= isApprox(lhs.velocities.data(), rhs.velocities.data(), lhs.velocities.size()); 27 | eq &= isApprox(lhs.accelerations.data(), rhs.accelerations.data(), lhs.accelerations.size()); 28 | eq &= isApprox(lhs.effort.data(), rhs.effort.data(), rhs.accelerations.size()); 29 | eq &= lhs.time_from_start == rhs.time_from_start; 30 | return eq; 31 | } 32 | 33 | template <> 34 | trajectory_msgs::JointTrajectory create() 35 | { 36 | const std::size_t n = 10; 37 | trajectory_msgs::JointTrajectory traj; 38 | traj.header = create(); 39 | traj.joint_names = std::vector(n, "test"); 40 | traj.points.resize(n); 41 | std::generate(traj.points.begin(), traj.points.end(), []() -> trajectory_msgs::JointTrajectoryPoint { 42 | return create(); 43 | }); 44 | 45 | return traj; 46 | } 47 | 48 | template <> 49 | bool equals(const trajectory_msgs::JointTrajectory& lhs, const trajectory_msgs::JointTrajectory& rhs) 50 | { 51 | bool eq = true; 52 | eq &= equals(lhs.header, rhs.header); 53 | eq &= lhs.joint_names == rhs.joint_names; 54 | eq &= equals(lhs.points, rhs.points); 55 | return eq; 56 | } 57 | -------------------------------------------------------------------------------- /test/utest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "std_msgs_test.h" 5 | #include "geometry_msgs_test.h" 6 | #include "trajectory_msgs_test.h" 7 | #include "shape_msgs_test.h" 8 | #include "sensor_msgs_test.h" 9 | 10 | const std::string YAML_EXT = "yaml"; 11 | const std::string BINARY_EXT = "msg"; 12 | 13 | std::string createFilename(const std::string& extension) 14 | { 15 | static int counter = 0; 16 | return "/tmp/" + std::to_string(++counter) + "_test." + extension; 17 | } 18 | 19 | template 20 | class SerializationTestFixture : public testing::Test 21 | { 22 | public: 23 | using testing::Test::Test; 24 | 25 | void runTest() 26 | { 27 | // YAML 28 | // Throwing version 29 | { 30 | const std::string filename = createFilename(YAML_EXT); 31 | T value = create(); 32 | EXPECT_NO_THROW(message_serialization::serialize(value, filename)); 33 | T new_value; 34 | EXPECT_NO_THROW(new_value = message_serialization::deserialize(filename)); 35 | EXPECT_TRUE(equals(value, new_value)); 36 | } 37 | 38 | // Non-throwing version 39 | { 40 | const std::string filename = createFilename(YAML_EXT); 41 | T value = create(); 42 | EXPECT_TRUE(message_serialization::serialize(filename, value)); 43 | T new_value; 44 | EXPECT_TRUE(message_serialization::deserialize(filename, new_value)); 45 | EXPECT_TRUE(equals(value, new_value)); 46 | } 47 | 48 | // Binary 49 | // Throwing version 50 | { 51 | const std::string filename = createFilename(BINARY_EXT); 52 | T value = create(); 53 | EXPECT_NO_THROW(message_serialization::serializeToBinary(value, filename)); 54 | T new_value; 55 | EXPECT_NO_THROW(new_value = message_serialization::deserializeFromBinary(filename)); 56 | EXPECT_TRUE(equals(value, new_value)); 57 | } 58 | 59 | // Non-throwing version 60 | { 61 | const std::string filename = createFilename(BINARY_EXT); 62 | T value = create(); 63 | EXPECT_TRUE(message_serialization::serializeToBinary(filename, value)); 64 | T new_value; 65 | EXPECT_TRUE(message_serialization::deserializeFromBinary(filename, new_value)); 66 | EXPECT_TRUE(equals(value, new_value)); 67 | } 68 | } 69 | }; 70 | 71 | using Implementations = testing::Types; 88 | 89 | TYPED_TEST_CASE(SerializationTestFixture, Implementations); 90 | 91 | TYPED_TEST(SerializationTestFixture, SerializationTest) 92 | { 93 | this->runTest(); 94 | } 95 | 96 | int main(int argc, char** argv) 97 | { 98 | testing::InitGoogleTest(&argc, argv); 99 | return RUN_ALL_TESTS(); 100 | } 101 | -------------------------------------------------------------------------------- /test/utilities.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | inline std::vector createRandomVector(const std::size_t n) 7 | { 8 | static std::mt19937 gen(0); 9 | static std::uniform_real_distribution dist; 10 | std::vector v(n); 11 | std::generate(v.begin(), v.end(), []() -> double { return dist(gen); }); 12 | return v; 13 | } 14 | 15 | inline void randomize(double* data, const std::size_t len) 16 | { 17 | static std::mt19937 gen(0); 18 | static std::uniform_real_distribution dist; 19 | std::generate(data, data + len, []() -> double { return dist(gen); }); 20 | } 21 | 22 | inline bool isApprox(const double* lhs, const double* rhs, const std::size_t len) 23 | { 24 | Eigen::Map map_lhs(lhs, len); 25 | Eigen::Map map_rhs(rhs, len); 26 | return map_lhs.isApprox(map_rhs); 27 | } 28 | 29 | /** @brief Specializable function for creating a serializable object with non-default values */ 30 | template 31 | T create(); 32 | 33 | /** @brief Specializable function for comparing the equality of two objects */ 34 | template 35 | bool equals(const T& lhs, const T& rhs); 36 | 37 | /** @brief Specializable function for comparing the equality of two vectors of objects */ 38 | template 39 | inline bool equals(const std::vector& lhs, const std::vector& rhs) 40 | { 41 | bool eq = lhs.size() == rhs.size(); 42 | if (eq) 43 | { 44 | for (std::size_t i = 0; i < lhs.size(); ++i) 45 | { 46 | eq &= equals(lhs[i], rhs[i]); 47 | } 48 | } 49 | return eq; 50 | } 51 | --------------------------------------------------------------------------------