Skip to content

Commit

Permalink
Add boost serialization for Environment commands and all underlying t…
Browse files Browse the repository at this point in the history 
…ypes (#726)

* Add serialization macros to tesseract_common

* Add serialization for tesseract_geometry primatives

* Add serialization for meshes and octree

* Add serialization for Link and Joint

* Add serialization for tesseract_common types

* Add serialization for SceneGraph and SceneState

* Add serialization for tesseract_srdf and tesseract_common types

* Add serialization for environment commands

* Fix bug in getCollisionObjectPairs
  • Loading branch information
@mpowelson
mpowelson authored Mar 24, 2022
1 parent bfa7976 commit fab4dc7
Show file tree
Hide file tree
Showing 117 changed files with 6,050 additions and 349 deletions.
4 changes: 2 additions & 2 deletions tesseract_collision/core/src/common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ std::vector<ObjectPairKey> getCollisionObjectPairs(const std::vector<std::string
for (std::size_t j = i + 1; j < active_links.size(); ++j)
{
const std::string& l2 = active_links[j];
if (acm != nullptr && !acm(l1, l2))
if (acm == nullptr || (acm != nullptr && !acm(l1, l2)))
clp.push_back(tesseract_collision::getObjectPairKey(l1, l2));
}
}
Expand All @@ -72,7 +72,7 @@ std::vector<ObjectPairKey> getCollisionObjectPairs(const std::vector<std::string
{
for (const auto& l2 : static_links)
{
if (acm != nullptr && !acm(l1, l2))
if (acm == nullptr || (acm != nullptr && !acm(l1, l2)))
clp.push_back(tesseract_collision::getObjectPairKey(l1, l2));
}
}
Expand Down
4 changes: 3 additions & 1 deletion tesseract_common/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,13 @@ add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENA

add_library(
${PROJECT_NAME}
src/allowed_collision_matrix.cpp
src/any.cpp
src/collision_margin_data.cpp
src/joint_state.cpp
src/manipulator_info.cpp
src/kinematic_limits.cpp
src/serialization.cpp
src/eigen_serialization.cpp
src/utils.cpp
src/resource_locator.cpp
src/types.cpp)
Expand Down
13 changes: 13 additions & 0 deletions tesseract_common/include/tesseract_common/allowed_collision_matrix.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/access.hpp>
#include <string>
#include <vector>
#include <memory>
Expand All @@ -23,6 +24,8 @@ namespace tesseract_common
using AllowedCollisionEntries =
std::unordered_map<tesseract_common::LinkNamesPair, std::string, tesseract_common::PairHash>;

bool operator==(const AllowedCollisionEntries& entries_1, const AllowedCollisionEntries& entries_2);

class AllowedCollisionMatrix
{
public:
Expand Down Expand Up @@ -125,13 +128,23 @@ class AllowedCollisionMatrix
os << "link=" << pair.first.first << " link=" << pair.first.second << " reason=" << pair.second << std::endl;
return os;
}
bool operator==(const AllowedCollisionMatrix& rhs) const;
bool operator!=(const AllowedCollisionMatrix& rhs) const;

private:
AllowedCollisionEntries lookup_table_;

friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int version); // NOLINT
};

} // namespace tesseract_common

#include <boost/serialization/export.hpp>
#include <boost/serialization/tracking.hpp>
BOOST_CLASS_EXPORT_KEY2(tesseract_common::AllowedCollisionMatrix, "AllowedCollisionMatrix")

#ifndef SWIG
namespace tesseract_scene_graph
{
Expand Down
34 changes: 9 additions & 25 deletions tesseract_common/include/tesseract_common/collision_margin_data.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/access.hpp>
#include <Eigen/Core>
#include <string>
#include <unordered_map>
Expand Down Expand Up @@ -237,31 +238,8 @@ class CollisionMarginData
}
}

bool operator==(const CollisionMarginData& other) const
{
bool ret_val = true;
ret_val &=
(tesseract_common::almostEqualRelativeAndAbs(default_collision_margin_, other.default_collision_margin_, 1e-5));
ret_val &= (tesseract_common::almostEqualRelativeAndAbs(max_collision_margin_, other.max_collision_margin_, 1e-5));
ret_val &= (lookup_table_.size() == other.lookup_table_.size());
if (ret_val)
{
for (const auto& pair : lookup_table_)
{
auto cp = other.lookup_table_.find(pair.first);
ret_val = (cp != other.lookup_table_.end());
if (!ret_val)
break;

ret_val = tesseract_common::almostEqualRelativeAndAbs(pair.second, cp->second, 1e-5);
if (!ret_val)
break;
}
}
return ret_val;
}

bool operator!=(const CollisionMarginData& rhs) const { return !operator==(rhs); }
bool operator==(const CollisionMarginData& rhs) const;
bool operator!=(const CollisionMarginData& rhs) const;

private:
/** @brief Stores the collision margin used if no pair-specific one is set */
Expand All @@ -283,7 +261,13 @@ class CollisionMarginData
max_collision_margin_ = p.second;
}
}
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int version); // NOLINT
};
} // namespace tesseract_common
#include <boost/serialization/export.hpp>
#include <boost/serialization/tracking.hpp>
BOOST_CLASS_EXPORT_KEY2(tesseract_common::CollisionMarginData, "CollisionMarginData")

#endif // TESSERACT_COMMON_COLLISION_MARGIN_DATA_H
147 changes: 147 additions & 0 deletions tesseract_common/include/tesseract_common/eigen_serialization.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,147 @@
/**
* @file serialization.h
* @brief Additional Boost serialization wrappers
* @details Supports the following
* - Eigen::VectorXd
* - Eigen::Isometry3d
* - Eigen::MatrixX2d
*
* @author Levi Armstrong
* @date February 21, 2021
* @version TODO
* @bug No known bugs
*
* @copyright Copyright (c) 2021, Southwest Research Institute
*
* @par License
* Software License Agreement (Apache License)
* @par
* 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
* @par
* 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 TESSERACT_COMMON_EIGEN_SERIALIZATION_H
#define TESSERACT_COMMON_EIGEN_SERIALIZATION_H

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <variant>
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/tracking.hpp>
#include <boost/serialization/tracking_enum.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

namespace boost
{
namespace serialization
{
/*****************************/
/****** Eigen::VectorXd ******/
/*****************************/
template <class Archive>
void save(Archive& ar, const Eigen::VectorXd& g, const unsigned int version); // NOLINT

template <class Archive>
void load(Archive& ar, Eigen::VectorXd& g, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, Eigen::VectorXd& g, const unsigned int version); // NOLINT

/*****************************/
/****** Eigen::Vector3d ******/
/*****************************/
template <class Archive>
void save(Archive& ar, const Eigen::Vector3d& g, const unsigned int version); // NOLINT

template <class Archive>
void load(Archive& ar, Eigen::Vector3d& g, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, Eigen::Vector3d& g, const unsigned int version); // NOLINT

/*****************************/
/****** Eigen::Vector4d ******/
/*****************************/
template <class Archive>
void save(Archive& ar, const Eigen::Vector4d& g, const unsigned int version); // NOLINT

template <class Archive>
void load(Archive& ar, Eigen::Vector4d& g, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, Eigen::Vector4d& g, const unsigned int version); // NOLINT

/*****************************/
/****** Eigen::VectorXi ******/
/*****************************/
template <class Archive>
void save(Archive& ar, const Eigen::VectorXi& g, const unsigned int version); // NOLINT

template <class Archive>
void load(Archive& ar, Eigen::VectorXi& g, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, Eigen::VectorXi& g, const unsigned int version); // NOLINT

/*****************************/
/****** Eigen::VectorXd ******/
/*****************************/

template <class Archive>
void save(Archive& ar, const Eigen::Isometry3d& g, const unsigned int version); // NOLINT

template <class Archive>
void load(Archive& ar, Eigen::Isometry3d& g, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, Eigen::Isometry3d& g, const unsigned int version); // NOLINT

/*****************************/
/****** Eigen::MatrixX2d *****/
/*****************************/
template <class Archive>
void save(Archive& ar, const Eigen::MatrixX2d& g, const unsigned int version); // NOLINT

template <class Archive>
void load(Archive& ar, Eigen::MatrixX2d& g, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, Eigen::MatrixX2d& g, const unsigned int version); // NOLINT

/*********************************************************/
/****** std::variant<std::string, Eigen::Isometry3d> *****/
/*********************************************************/
template <class Archive>
void save(Archive& ar, const std::variant<std::string, Eigen::Isometry3d>& g, const unsigned int version); // NOLINT

template <class Archive>
void load(Archive& ar, std::variant<std::string, Eigen::Isometry3d>& g, const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, std::variant<std::string, Eigen::Isometry3d>& g, const unsigned int version); // NOLINT

} // namespace serialization
} // namespace boost

// Set the tracking to track_never for all Eigen types.
BOOST_CLASS_TRACKING(Eigen::VectorXd, boost::serialization::track_never);
BOOST_CLASS_TRACKING(Eigen::Vector3d, boost::serialization::track_never);
BOOST_CLASS_TRACKING(Eigen::Vector4d, boost::serialization::track_never);
BOOST_CLASS_TRACKING(Eigen::VectorXi, boost::serialization::track_never);
BOOST_CLASS_TRACKING(Eigen::Isometry3d, boost::serialization::track_never);
BOOST_CLASS_TRACKING(Eigen::MatrixX2d, boost::serialization::track_never);

#endif // TESSERACT_COMMON_SERIALIZATION_H
Loading

0 comments on commit fab4dc7

Please sign in to comment.