diff --git a/dependencies.rosinstall b/dependencies.rosinstall index c13e782..adf4c7f 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -21,3 +21,9 @@ local-name: cmake_common_scripts uri: 'https://github.com/ros-industrial/cmake_common_scripts.git' version: 15450c289e1a92c51e9b6d953f4d07d7e7ccaa18 + +# Message serialization +- git: + local-name: message_serialization + uri: 'https://github.com/swri-robotics/message_serialization.git' + version: 0.1.0 \ No newline at end of file diff --git a/opp_area_selection/src/selection_artist.cpp b/opp_area_selection/src/selection_artist.cpp index 28ac0d9..5b4ae74 100644 --- a/opp_area_selection/src/selection_artist.cpp +++ b/opp_area_selection/src/selection_artist.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include @@ -291,7 +291,7 @@ bool SelectionArtist::collectROIPointsCb(opp_msgs::GetROISelectionRequest& req, pcl::fromROSMsg(req.input_cloud, *cloud); AreaSelectorParameters params; - bool success = opp_msgs_serialization::deserialize(area_selection_config_file, params); + bool success = message_serialization::deserialize(area_selection_config_file, params); if (!success) { ROS_ERROR_STREAM("Could not load area selection config from: " << area_selection_config_file); diff --git a/opp_database/CMakeLists.txt b/opp_database/CMakeLists.txt index 88cc6c7..076a48d 100644 --- a/opp_database/CMakeLists.txt +++ b/opp_database/CMakeLists.txt @@ -13,9 +13,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(Qt5 REQUIRED COMPONENTS Sql) find_package(Boost REQUIRED) -# Prevents certain runtime errors in Ubuntu 18.04 -add_definitions(${PCL_DEFINITIONS}) - catkin_package( INCLUDE_DIRS include @@ -27,8 +24,9 @@ catkin_package( opp_msgs opp_msgs_serialization DEPENDS - Qt5 Boost + CFG_EXTRAS + ${PROJECT_NAME}-extras.cmake ) include_directories( @@ -46,7 +44,6 @@ target_link_libraries(${PROJECT_NAME}_cpp Qt5::Sql ) - ############# ## Install ## ############# diff --git a/opp_database/cmake/opp_database-extras.cmake b/opp_database/cmake/opp_database-extras.cmake new file mode 100644 index 0000000..06a3e4d --- /dev/null +++ b/opp_database/cmake/opp_database-extras.cmake @@ -0,0 +1 @@ +find_package(Qt5 REQUIRED COMPONENTS Sql) diff --git a/opp_database/src/opp_database_interface_cpp.cpp b/opp_database/src/opp_database_interface_cpp.cpp index 011fae8..afef7b3 100644 --- a/opp_database/src/opp_database_interface_cpp.cpp +++ b/opp_database/src/opp_database_interface_cpp.cpp @@ -26,7 +26,7 @@ #include -#include +#include #include namespace opp_db @@ -66,7 +66,7 @@ long int ROSDatabaseInterface::addJobToDatabase(const opp_msgs::Job& job, std::s std::string filepath = save_dir_ + "/job_" + job.name + "_" + boost::posix_time::to_iso_string(ros::Time::now().toBoost()) + ".yaml"; - bool success = opp_msgs_serialization::serialize(filepath, job.paths); + bool success = message_serialization::serialize(filepath, job.paths); if (!success) { message = "Failed to save tool paths to file at '" + filepath + "'"; @@ -92,7 +92,7 @@ long int ROSDatabaseInterface::addPartToDatabase(const opp_msgs::Part& part, std // touchoff_points std::string filepath = save_dir_ + "/touch_pts_" + part.name + "_" + time + ".yaml"; - bool success = opp_msgs_serialization::serialize(filepath, part.touch_points); + bool success = message_serialization::serialize(filepath, part.touch_points); if (!success) { message = "Failed to save touchoff points to file '" + filepath + "'"; @@ -103,7 +103,7 @@ long int ROSDatabaseInterface::addPartToDatabase(const opp_msgs::Part& part, std // verification_points filepath = save_dir_ + "/verification_points_" + part.name + "_" + time + ".yaml"; - success = opp_msgs_serialization::serialize(filepath, part.verification_points); + success = message_serialization::serialize(filepath, part.verification_points); if (!success) { message = "Failed to save verification points to file '" + filepath + "'"; @@ -147,7 +147,7 @@ bool ROSDatabaseInterface::getJobFromDatabase(const unsigned int job_id, std::st job.part_id = result.value("part_id").toUInt(); const std::string file_path = result.value("paths").toString().toStdString(); - bool success = opp_msgs_serialization::deserialize(file_path, job.paths); + bool success = message_serialization::deserialize(file_path, job.paths); if (!success) { message = "Failed to load tool paths file from '" + file_path + "'"; @@ -191,7 +191,7 @@ bool ROSDatabaseInterface::getAllJobsFromDatabase(const unsigned int part_id, j.part_id = result.value("part_id").toUInt(); const std::string filepath = result.value("paths").toString().toStdString(); - bool success = opp_msgs_serialization::deserialize(filepath, j.paths); + bool success = message_serialization::deserialize(filepath, j.paths); if (!success) { message = "Failed to load tool path file from '" + filepath + "'"; @@ -237,7 +237,7 @@ bool ROSDatabaseInterface::getPartFromDatabase(const unsigned int part_id, std:: part.mesh_resource = result.value("mesh_resource").toString().toStdString(); const std::string touchoff_pts_filename = result.value("touchoff_points").toString().toStdString(); - bool success = opp_msgs_serialization::deserialize(touchoff_pts_filename, part.touch_points); + bool success = message_serialization::deserialize(touchoff_pts_filename, part.touch_points); if (!success) { message = "Failed to load touchoff points from '" + touchoff_pts_filename + "'"; @@ -245,7 +245,7 @@ bool ROSDatabaseInterface::getPartFromDatabase(const unsigned int part_id, std:: } const std::string verification_pts_filename = result.value("verification_points").toString().toStdString(); - success = opp_msgs_serialization::deserialize(verification_pts_filename, part.verification_points); + success = message_serialization::deserialize(verification_pts_filename, part.verification_points); if (!success) { message = "Failed to load verification points from '" + verification_pts_filename + "'"; @@ -285,7 +285,7 @@ bool ROSDatabaseInterface::getAllPartsFromDatabase(std::string& message, std::ma p.mesh_resource = result.value("mesh_resource").toString().toStdString(); const std::string touch_pts_filename = result.value("touchoff_points").toString().toStdString(); - bool success = opp_msgs_serialization::deserialize(touch_pts_filename, p.touch_points); + bool success = message_serialization::deserialize(touch_pts_filename, p.touch_points); if (!success) { message = "Failed to load touchoff points from '" + touch_pts_filename + "'"; @@ -293,7 +293,7 @@ bool ROSDatabaseInterface::getAllPartsFromDatabase(std::string& message, std::ma } const std::string verification_pts_filename = result.value("verification_points").toString().toStdString(); - success = opp_msgs_serialization::deserialize(verification_pts_filename, p.verification_points); + success = message_serialization::deserialize(verification_pts_filename, p.verification_points); if (!success) { message = "Failed to load verification points from '" + verification_pts_filename + "'"; diff --git a/opp_gui/CMakeLists.txt b/opp_gui/CMakeLists.txt index aa7fa1f..03af7a0 100644 --- a/opp_gui/CMakeLists.txt +++ b/opp_gui/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS opp_area_selection opp_msgs opp_database + pcl_ros rviz shape_msgs std_srvs @@ -27,9 +28,6 @@ find_package(Qt5 REQUIRED COMPONENTS Sql ) -find_package(PCL 1.9 REQUIRED) -add_definitions(${PCL_DEFINITIONS}) - catkin_package( INCLUDE_DIRS include @@ -49,13 +47,13 @@ catkin_package( opp_area_selection opp_msgs opp_database + pcl_ros rviz shape_msgs std_srvs visualization_msgs - DEPENDS - Qt5 - PCL + CFG_EXTRAS + ${PROJECT_NAME}-extras.cmake ) ########### @@ -66,7 +64,6 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${CMAKE_CURRENT_BINARY_DIR} - ${PCL_INCLUDE_DIRS} ) set(opp_gui_UIS @@ -129,7 +126,6 @@ target_link_libraries(${PROJECT_NAME}_utils ${catkin_LIBRARIES} ${PCL_LIBRARIES} Qt5::Core - Qt5::Sql ) # Qt Widgets @@ -143,6 +139,7 @@ target_link_libraries(${PROJECT_NAME}_widgets ${PCL_LIBRARIES} ${PROJECT_NAME}_utils Qt5::Widgets + Qt5::Sql ) # Rviz Panel @@ -194,7 +191,6 @@ target_link_libraries(touch_point_editor_demo_app ## Install ## ############# -# Mark executables and/or libraries for installation install( TARGETS ${PROJECT_NAME}_panels @@ -209,15 +205,14 @@ install( RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -# Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -# Mark other files for installation (e.g. launch and bag files, etc.) install(DIRECTORY ui/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ui ) + install( FILES plugin_description.xml diff --git a/opp_gui/cmake/opp_gui-extras.cmake b/opp_gui/cmake/opp_gui-extras.cmake new file mode 100644 index 0000000..bf8253c --- /dev/null +++ b/opp_gui/cmake/opp_gui-extras.cmake @@ -0,0 +1 @@ +find_package(Qt5 REQUIRED COMPONENTS Core Widgets Sql) diff --git a/opp_gui/package.xml b/opp_gui/package.xml index 859cf71..ab5c278 100644 --- a/opp_gui/package.xml +++ b/opp_gui/package.xml @@ -31,6 +31,7 @@ opp_area_selection opp_msgs opp_database + pcl_ros rviz shape_msgs std_srvs diff --git a/opp_msgs_serialization/CMakeLists.txt b/opp_msgs_serialization/CMakeLists.txt index 4b75006..3dfcb17 100644 --- a/opp_msgs_serialization/CMakeLists.txt +++ b/opp_msgs_serialization/CMakeLists.txt @@ -11,14 +11,9 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs std_msgs - roscpp_serialization + message_serialization ) -find_package(yaml-cpp REQUIRED) - -# Prevents certain runtime errors in Ubuntu 18.04 -add_definitions(${PCL_DEFINITIONS}) - catkin_package( INCLUDE_DIRS include @@ -31,8 +26,7 @@ catkin_package( noether_msgs roscpp opp_msgs - DEPENDS - YAML_CPP + message_serialization ) ########### @@ -42,7 +36,6 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIRS} ) ############# @@ -64,6 +57,5 @@ if(CATKIN_ENABLE_TESTING) ) target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} - yaml-cpp ) endif() diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h b/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h deleted file mode 100644 index 8d2cef6..0000000 --- a/opp_msgs_serialization/include/opp_msgs_serialization/binary_serialization.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H -#define OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H - -#include - -#include - -namespace opp_msgs_serialization -{ -template -inline bool serializeToBinary(const std::string& file, const T& message) -{ - uint32_t serial_size = ros::serialization::serializationLength(message); - boost::shared_array buffer(new uint8_t[serial_size]); - ros::serialization::OStream stream(buffer.get(), serial_size); - ros::serialization::serialize(stream, message); - - std::ofstream ofs(file, std::ios::out | std::ios::binary); - if (ofs) - { - ofs.write((char*)buffer.get(), serial_size); - return ofs.good(); - } - - return false; -} - -template -inline bool deserializeFromBinary(const std::string& file, T& message) -{ - std::ifstream ifs(file, std::ios::in | std::ios::binary); - if (!ifs) - { - return false; - } - - ifs.seekg(0, std::ios::end); - std::streampos end = ifs.tellg(); - ifs.seekg(0, std::ios::beg); - std::streampos begin = ifs.tellg(); - uint32_t file_size = end - begin; - - boost::shared_array ibuffer(new uint8_t[file_size]); - ifs.read((char*)ibuffer.get(), file_size); - ros::serialization::IStream istream(ibuffer.get(), file_size); - ros::serialization::deserialize(istream, message); - - ifs.close(); - return true; -} - -} // namespace opp_msgs_serialization - -#endif // OPP_MSGS_SERIALIZATION_BINARY_SERIALIZATION_H diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h deleted file mode 100644 index 7117971..0000000 --- a/opp_msgs_serialization/include/opp_msgs_serialization/eigen_yaml.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright 2018 Southwest Research Institute - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef OPP_MSGS_SERIALIZATION_EIGEN_YAML_H -#define OPP_MSGS_SERIALIZATION_EIGEN_YAML_H - -#include - -#include "opp_msgs_serialization/geometry_msgs_yaml.h" - -namespace YAML -{ -template <> -struct convert -{ - static Node encode(const Eigen::Affine3d& rhs) - { - geometry_msgs::Pose msg; - tf::poseEigenToMsg(rhs, msg); - Node node = Node(msg); - return node; - } - - static bool decode(const Node& node, Eigen::Affine3d& rhs) - { - geometry_msgs::Pose msg; - msg = node.as(); - tf::poseMsgToEigen(msg, rhs); - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const Eigen::Vector3d& rhs) - { - geometry_msgs::Point msg; - tf::pointEigenToMsg(rhs, msg); - Node node = Node(msg); - return node; - } - - static bool decode(const Node& node, Eigen::Vector3d& rhs) - { - geometry_msgs::Point msg; - msg = node.as(); - tf::pointMsgToEigen(msg, rhs); - return true; - } -}; - -} // namespace YAML - -#endif // OPP_MSGS_SERIALIZATION_EIGEN_YAML_H diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h deleted file mode 100644 index 6f58a2a..0000000 --- a/opp_msgs_serialization/include/opp_msgs_serialization/geometry_msgs_yaml.h +++ /dev/null @@ -1,225 +0,0 @@ -/* - * Copyright 2018 Southwest Research Institute - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML -#define OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML - -#include -#include -#include - -#include "opp_msgs_serialization/std_msgs_yaml.h" - -namespace YAML -{ -template <> -struct convert -{ - static Node encode(const geometry_msgs::Vector3& rhs) - { - Node node; - node["x"] = rhs.x; - node["y"] = rhs.y; - node["z"] = rhs.z; - - return node; - } - - static bool decode(const Node& node, geometry_msgs::Vector3& rhs) - { - if (node.size() != 3) - return false; - - rhs.x = node["x"].as(); - rhs.y = node["y"].as(); - rhs.z = node["z"].as(); - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const geometry_msgs::Point& rhs) - { - Node node; - node["x"] = rhs.x; - node["y"] = rhs.y; - node["z"] = rhs.z; - return node; - } - - static bool decode(const Node& node, geometry_msgs::Point& rhs) - { - if (node.size() != 3) - return false; - - rhs.x = node["x"].as(); - rhs.y = node["y"].as(); - rhs.z = node["z"].as(); - - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const geometry_msgs::Quaternion& rhs) - { - Node node; - node["x"] = rhs.x; - node["y"] = rhs.y; - node["z"] = rhs.z; - node["w"] = rhs.w; - - return node; - } - - static bool decode(const Node& node, geometry_msgs::Quaternion& rhs) - { - if (node.size() != 4) - return false; - - rhs.x = node["x"].as(); - rhs.y = node["y"].as(); - rhs.z = node["z"].as(); - rhs.w = node["w"].as(); - - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const geometry_msgs::Pose& rhs) - { - Node node; - node["position"] = rhs.position; - node["orientation"] = rhs.orientation; - - return node; - } - - static bool decode(const Node& node, geometry_msgs::Pose& rhs) - { - if (node.size() != 2) - return false; - - rhs.position = node["position"].as(); - rhs.orientation = node["orientation"].as(); - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const geometry_msgs::PoseStamped& rhs) - { - Node node; - node["header"] = rhs.header; - node["pose"] = rhs.pose; - - return node; - } - - static bool decode(const Node& node, geometry_msgs::PoseStamped& rhs) - { - if (node.size() != 2) - return false; - - rhs.header = node["header"].as(); - rhs.pose = node["pose"].as(); - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const geometry_msgs::PoseArray& rhs) - { - Node node; - node["header"] = rhs.header; - node["poses"] = rhs.poses; - - return node; - } - - static bool decode(const Node& node, geometry_msgs::PoseArray& rhs) - { - if (node.size() != 2) - return false; - - rhs.header = node["header"].as(); - rhs.poses = node["poses"].as >(); - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const geometry_msgs::Transform& rhs) - { - Node node; - node["rotation"] = rhs.rotation; - node["translation"] = rhs.translation; - - return node; - } - - static bool decode(const Node& node, geometry_msgs::Transform& rhs) - { - if (node.size() != 2) - return false; - - rhs.rotation = node["rotation"].as(); - rhs.translation = node["translation"].as(); - - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const geometry_msgs::TransformStamped& rhs) - { - Node node; - node["header"] = rhs.header; - node["child_frame_id"] = rhs.child_frame_id; - node["transform"] = rhs.transform; - - return node; - } - - static bool decode(const Node& node, geometry_msgs::TransformStamped& rhs) - { - if (node.size() != 3) - return false; - - rhs.header = node["header"].as(); - rhs.child_frame_id = node["child_frame_id"].as(); - rhs.transform = node["transform"].as(); - return true; - } -}; - -} // namespace YAML - -#endif // OPP_MSGS_SERIALIZATION_GEOMETRY_MSGS_YAML diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h index 6396f2d..eeec05b 100644 --- a/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h +++ b/opp_msgs_serialization/include/opp_msgs_serialization/opp_msgs_yaml.h @@ -22,8 +22,8 @@ #include #include #include -#include "opp_msgs_serialization/geometry_msgs_yaml.h" -#include "opp_msgs_serialization/sensor_msgs_yaml.h" +#include +#include namespace YAML { diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h deleted file mode 100644 index 3532e50..0000000 --- a/opp_msgs_serialization/include/opp_msgs_serialization/sensor_msgs_yaml.h +++ /dev/null @@ -1,147 +0,0 @@ -/* - * Copyright 2018 Southwest Research Institute - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML -#define OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML - -#include -#include - -#include "opp_msgs_serialization/std_msgs_yaml.h" - -namespace YAML -{ -template <> -struct convert -{ - static Node encode(const sensor_msgs::RegionOfInterest& rhs) - { - Node node; - - node["x_offset"] = rhs.x_offset; - node["y_offset"] = rhs.y_offset; - node["height"] = rhs.height; - node["width"] = rhs.width; - node["do_rectify"] = rhs.do_rectify; - - return node; - } - - static bool decode(const Node& node, sensor_msgs::RegionOfInterest& rhs) - { - if (node.size() != 5) - return false; - - rhs.x_offset = node["x_offset"].as(); - rhs.y_offset = node["y_offset"].as(); - rhs.height = node["height"].as(); - rhs.width = node["width"].as(); - rhs.do_rectify = node["do_rectify"].as(); - - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const sensor_msgs::CameraInfo& rhs) - { - Node node; - - node["header"] = rhs.header; - node["height"] = rhs.height; - node["width"] = rhs.width; - node["distortion_model"] = rhs.distortion_model; - - std::vector K_vec, R_vec, P_vec; - - std::copy_n(rhs.K.begin(), rhs.K.size(), std::back_inserter(K_vec)); - std::copy_n(rhs.R.begin(), rhs.R.size(), std::back_inserter(R_vec)); - std::copy_n(rhs.P.begin(), rhs.P.size(), std::back_inserter(P_vec)); - - node["D"] = rhs.D; - node["K"] = K_vec; - node["R"] = R_vec; - node["P"] = P_vec; - node["binning_x"] = rhs.binning_x; - node["binning_y"] = rhs.binning_y; - node["roi"] = rhs.roi; - - return node; - } - - static bool decode(const Node& node, sensor_msgs::CameraInfo& rhs) - { - if (node.size() != 11) - return false; - - rhs.header = node["header"].as(); - rhs.height = node["height"].as(); - rhs.width = node["width"].as(); - rhs.distortion_model = node["distortion_model"].as(); - rhs.D = node["D"].as(); - - std::vector K_vec, R_vec, P_vec; - K_vec = node["K"].as(); - R_vec = node["R"].as(); - P_vec = node["P"].as(); - - std::copy_n(K_vec.begin(), K_vec.size(), rhs.K.begin()); - std::copy_n(R_vec.begin(), R_vec.size(), rhs.R.begin()); - std::copy_n(P_vec.begin(), P_vec.size(), rhs.P.begin()); - - rhs.binning_x = node["binning_x"].as(); - rhs.binning_y = node["binning_y"].as(); - rhs.roi = node["roi"].as(); - - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const sensor_msgs::JointState& rhs) - { - Node node; - - node["header"] = rhs.header; - node["name"] = rhs.name; - node["position"] = rhs.position; - node["velocity"] = rhs.velocity; - node["effort"] = rhs.effort; - - return node; - } - - static bool decode(const Node& node, sensor_msgs::JointState& rhs) - { - if (node.size() != 5) - return false; - - rhs.header = node["header"].as(); - rhs.name = node["name"].as(); - rhs.position = node["position"].as(); - rhs.velocity = node["velocity"].as(); - rhs.effort = node["effort"].as(); - - return true; - } -}; - -} // namespace YAML - -#endif // OPP_MSGS_SERIALIZATION_SENSOR_MSGS_YAML diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h b/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h deleted file mode 100644 index c11d605..0000000 --- a/opp_msgs_serialization/include/opp_msgs_serialization/serialize.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright 2018 Southwest Research Institute - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef OPP_MSGS_SERIALIZATION_SERIALIZE_H -#define OPP_MSGS_SERIALIZATION_SERIALIZE_H - -#include - -#include - -#include - -namespace opp_msgs_serialization -{ -template -bool serialize(const std::string& file, const T& val) -{ - std::ofstream ofh(file); - if (!ofh) - { - return false; - } - - YAML::Node n = YAML::Node(val); - ofh << n; - return true; -} - -template -bool deserialize(const std::string& file, T& val) -{ - try - { - YAML::Node node; - node = YAML::LoadFile(file); - val = node.as(); - return true; - } - catch (const YAML::Exception& ex) - { - ROS_ERROR_STREAM("An exception was thrown while processing file: " << file); - ROS_ERROR_STREAM(ex.what()); - return false; - } -} - -} // namespace opp_msgs_serialization - -#endif // OPP_MSGS_SERIALIZATION_SERIALIZE_H diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h deleted file mode 100644 index 5ae7600..0000000 --- a/opp_msgs_serialization/include/opp_msgs_serialization/std_msgs_yaml.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright 2018 Southwest Research Institute - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef OPP_MSGS_SERIALIZATION_STD_MSGS_YAML -#define OPP_MSGS_SERIALIZATION_STD_MSGS_YAML - -#include - -#include - -namespace YAML -{ -template <> -struct convert -{ - static Node encode(const ros::Time& rhs) - { - Node node; - node["sec"] = rhs.sec; - node["nsec"] = rhs.nsec; - return node; - } - - static bool decode(const Node& node, ros::Time& rhs) - { - if (node.size() != 2) - return false; - - rhs.sec = node["sec"].as(); - rhs.nsec = node["nsec"].as(); - - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const std_msgs::Header& rhs) - { - Node node; - node["seq"] = rhs.seq; - node["stamp"] = rhs.stamp; - node["frame_id"] = rhs.frame_id; - return node; - } - - static bool decode(const Node& node, std_msgs::Header& rhs) - { - if (node.size() != 3) - return false; - - rhs.seq = node["seq"].as(); - rhs.stamp = node["stamp"].as(); - rhs.frame_id = node["frame_id"].as(); - - return true; - } -}; - -// TODO: Duration - -} // namespace YAML - -#endif // OPP_MSGS_SERIALIZATION_STD_MSGS_YAML diff --git a/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h b/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h deleted file mode 100644 index 01713eb..0000000 --- a/opp_msgs_serialization/include/opp_msgs_serialization/trajectory_msgs_yaml.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright 2018 Southwest Research Institute - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML -#define OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML - -#include - -#include "opp_msgs_serialization/std_msgs_yaml.h" - -namespace YAML -{ -template <> -struct convert -{ - static Node encode(const trajectory_msgs::JointTrajectoryPoint& rhs) - { - Node node; - node["positions"] = rhs.positions; - node["velocities"] = rhs.velocities; - node["accelerations"] = rhs.accelerations; - node["effort"] = rhs.effort; - node["time_from_start"] = rhs.time_from_start.toSec(); - - return node; - } - - static bool decode(const Node& node, trajectory_msgs::JointTrajectoryPoint& rhs) - { - if (node.size() != 5) - return false; - - rhs.positions = node["positions"].as >(); - rhs.velocities = node["velocities"].as >(); - rhs.accelerations = node["accelerations"].as >(); - rhs.effort = node["effort"].as >(); - rhs.time_from_start = ros::Duration(node["time_from_start"].as()); - - return true; - } -}; - -template <> -struct convert -{ - static Node encode(const trajectory_msgs::JointTrajectory& rhs) - { - Node node; - node["header"] = rhs.header; - node["joint_names"] = rhs.joint_names; - node["points"] = rhs.points; - return node; - } - - static bool decode(const Node& node, trajectory_msgs::JointTrajectory& rhs) - { - if (node.size() != 3) - return false; - - rhs.header = node["header"].as(); - rhs.joint_names = node["joint_names"].as >(); - rhs.points = node["points"].as >(); - - return true; - } -}; - -} // namespace YAML - -#endif // OPP_MSGS_SERIALIZATION_TRAJECTORY_MSGS_YAML diff --git a/opp_msgs_serialization/package.xml b/opp_msgs_serialization/package.xml index 68f628e..c38dff6 100644 --- a/opp_msgs_serialization/package.xml +++ b/opp_msgs_serialization/package.xml @@ -18,6 +18,7 @@ sensor_msgs std_msgs roscpp_serialization + message_serialization rostest diff --git a/opp_msgs_serialization/test/serialization_test.cpp b/opp_msgs_serialization/test/serialization_test.cpp index 8417972..21c8f3f 100644 --- a/opp_msgs_serialization/test/serialization_test.cpp +++ b/opp_msgs_serialization/test/serialization_test.cpp @@ -15,7 +15,7 @@ */ #include -#include +#include #include TEST(opp_serialization, process_type) @@ -24,11 +24,11 @@ TEST(opp_serialization, process_type) opp_msgs::ProcessType orig_msg; orig_msg.val = opp_msgs::ProcessType::PROCESS_PAINT; - bool result = opp_msgs_serialization::serialize(file, orig_msg); + bool result = message_serialization::serialize(file, orig_msg); EXPECT_TRUE(result); opp_msgs::ProcessType new_msg; - bool load_result = opp_msgs_serialization::deserialize(file, new_msg); + bool load_result = message_serialization::deserialize(file, new_msg); EXPECT_TRUE(load_result); bool eq = (orig_msg.val == new_msg.val); @@ -49,11 +49,11 @@ TEST(opp_serialization, touch_point) orig_msg.transform.pose.orientation.y = 1.0; orig_msg.transform.pose.orientation.z = 1.0; - bool save_result = opp_msgs_serialization::serialize(file, orig_msg); + bool save_result = message_serialization::serialize(file, orig_msg); EXPECT_TRUE(save_result); opp_msgs::TouchPoint new_msg; - bool load_result = opp_msgs_serialization::deserialize(file, new_msg); + bool load_result = message_serialization::deserialize(file, new_msg); EXPECT_TRUE(load_result); EXPECT_EQ(new_msg.name, orig_msg.name); @@ -101,11 +101,11 @@ TEST(opp_serialization, tool_path) orig_msg.paths.push_back(std::move(segment)); } - bool result = opp_msgs_serialization::serialize(file, orig_msg); + bool result = message_serialization::serialize(file, orig_msg); EXPECT_TRUE(result); opp_msgs::ToolPath new_msg; - bool load_result = opp_msgs_serialization::deserialize(file, new_msg); + bool load_result = message_serialization::deserialize(file, new_msg); EXPECT_TRUE(load_result); EXPECT_EQ(new_msg.process_type.val, orig_msg.process_type.val);