Skip to content

Commit b1ac19d

Browse files
Levi ArmstrongLevi-Armstrong
Levi Armstrong
authored and
Levi-Armstrong
committed
Start to adding boost serialization support
1 parent 048fe97 commit b1ac19d

File tree

9 files changed

+474
-37
lines changed

9 files changed

+474
-37
lines changed

‎tesseract_common/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ endif()
1212
include(cmake/tesseract_macros.cmake)
1313
list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/")
1414

15-
find_package(Boost REQUIRED COMPONENTS system filesystem program_options)
15+
find_package(Boost REQUIRED COMPONENTS system filesystem program_options serialization)
1616
find_package(Eigen3 REQUIRED)
1717
find_package(TinyXML2 REQUIRED)
1818

‎tesseract_common/cmake/tesseract_common-config.cmake.in‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,9 @@ include(CMakeFindDependencyMacro)
1010
find_dependency(Eigen3)
1111
find_dependency(TinyXML2)
1212
if(${CMAKE_VERSION} VERSION_LESS "3.10.0")
13-
find_package(Boost REQUIRED COMPONENTS system filesystem program_options)
13+
find_package(Boost REQUIRED COMPONENTS system filesystem program_options serialization)
1414
else()
15-
find_dependency(Boost COMPONENTS system filesystem program_options)
15+
find_dependency(Boost COMPONENTS system filesystem program_options serialization)
1616
endif()
1717

1818
find_dependency(console_bridge)

‎tesseract_common/include/tesseract_common/joint_state.h‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
/**
2+
* @file joint_state.h
3+
* @brief Tesseract Joint State
4+
*
5+
* @author Levi Armstrong
6+
* @date January 18, 2018
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @copyright Copyright (c) 2017, Southwest Research Institute
11+
*
12+
* @par License
13+
* Software License Agreement (Apache License)
14+
* @par
15+
* Licensed under the Apache License, Version 2.0 (the "License");
16+
* you may not use this file except in compliance with the License.
17+
* You may obtain a copy of the License at
18+
* http://www.apache.org/licenses/LICENSE-2.0
19+
* @par
20+
* Unless required by applicable law or agreed to in writing, software
21+
* distributed under the License is distributed on an "AS IS" BASIS,
22+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
23+
* See the License for the specific language governing permissions and
24+
* limitations under the License.
25+
*/
26+
#ifndef TESSERACT_COMMON_JOINT_STATE_H
27+
#define TESSERACT_COMMON_JOINT_STATE_H
28+
29+
#include <tesseract_common/macros.h>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31+
#include <Eigen/Dense>
32+
#include <vector>
33+
#include <boost/serialization/base_object.hpp>
34+
#include <boost/serialization/nvp.hpp>
35+
#include <boost/serialization/string.hpp>
36+
#include <boost/serialization/vector.hpp>
37+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
38+
39+
#include <tesseract_common/utils.h>
40+
#include <tesseract_common/serialization.h>
41+
42+
namespace tesseract_common
43+
{
44+
struct JointState
45+
{
46+
JointState() = default;
47+
JointState(std::vector<std::string> joint_names, Eigen::VectorXd position)
48+
: joint_names(std::move(joint_names)), position(std::move(position))
49+
{
50+
}
51+
52+
/** @brief The joint corresponding to the position vector. */
53+
std::vector<std::string> joint_names;
54+
55+
/** @brief The joint position at the waypoint */
56+
Eigen::VectorXd position;
57+
58+
/** @brief The velocity at the waypoint (optional) */
59+
Eigen::VectorXd velocity;
60+
61+
/** @brief The Acceleration at the waypoint (optional) */
62+
Eigen::VectorXd acceleration;
63+
64+
/** @brief The Effort at the waypoint (optional) */
65+
Eigen::VectorXd effort;
66+
67+
/** @brief The Time from start at the waypoint (optional) */
68+
double time{ 0 };
69+
70+
bool operator==(const JointState& other) const
71+
{
72+
bool ret_val = true;
73+
ret_val &= (joint_names == other.joint_names);
74+
ret_val &= (position.isApprox(other.position, 1e-5));
75+
ret_val &= (velocity.isApprox(other.velocity, 1e-5));
76+
ret_val &= (acceleration.isApprox(other.acceleration, 1e-5));
77+
ret_val &= (effort.isApprox(other.effort, 1e-5));
78+
ret_val &= (tesseract_common::almostEqualRelativeAndAbs(time, other.time, 1e-5));
79+
return ret_val;
80+
}
81+
82+
private:
83+
friend class boost::serialization::access;
84+
template <class Archive>
85+
void serialize(Archive& ar, const unsigned int /*version*/)
86+
{
87+
ar& BOOST_SERIALIZATION_NVP(joint_names);
88+
ar& BOOST_SERIALIZATION_NVP(position);
89+
ar& BOOST_SERIALIZATION_NVP(velocity);
90+
ar& BOOST_SERIALIZATION_NVP(acceleration);
91+
ar& BOOST_SERIALIZATION_NVP(effort);
92+
ar& BOOST_SERIALIZATION_NVP(time);
93+
}
94+
};
95+
96+
/** @brief Represents a joint trajectory */
97+
using JointTrajectory = std::vector<JointState>;
98+
} // namespace tesseract_common
99+
100+
#endif // TESSERACT_COMMON_JOINT_STATE_H

‎tesseract_common/include/tesseract_common/manipulator_info.h‎

Lines changed: 27 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,9 +31,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3131
#include <vector>
3232
#include <Eigen/Geometry>
3333
#include <tinyxml2.h>
34+
#include <boost/serialization/base_object.hpp>
35+
#include <boost/serialization/nvp.hpp>
3436
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3537

3638
#include <tesseract_common/utils.h>
39+
#include <tesseract_common/serialization.h>
3740

3841
namespace tesseract_common
3942
{
@@ -125,7 +128,7 @@ class ToolCenterPoint
125128
bool ret_val = true;
126129
ret_val &= (type_ == other.type_);
127130
ret_val &= (name_ == other.name_);
128-
ret_val &= (transform_.isApprox(transform_, 1e-5));
131+
ret_val &= (transform_.isApprox(other.transform_, 1e-5));
129132
ret_val &= (external_ == other.external_);
130133
ret_val &= (external_frame_ == other.external_frame_);
131134
return ret_val;
@@ -134,7 +137,7 @@ class ToolCenterPoint
134137
protected:
135138
int type_{ 0 };
136139
std::string name_;
137-
Eigen::Isometry3d transform_;
140+
Eigen::Isometry3d transform_{ Eigen::Isometry3d::Identity() };
138141

139142
/**
140143
* @brief The external TCP is used when the robot is holding the part versus the tool.
@@ -149,6 +152,17 @@ class ToolCenterPoint
149152
* world.
150153
*/
151154
std::string external_frame_;
155+
156+
friend class boost::serialization::access;
157+
template <class Archive>
158+
void serialize(Archive& ar, const unsigned int /*version*/)
159+
{
160+
ar& boost::serialization::make_nvp("type", type_);
161+
ar& boost::serialization::make_nvp("name", name_);
162+
ar& boost::serialization::make_nvp("transform", transform_);
163+
ar& boost::serialization::make_nvp("external", external_);
164+
ar& boost::serialization::make_nvp("external_frame", external_frame_);
165+
}
152166
};
153167

154168
/**
@@ -200,6 +214,17 @@ struct ManipulatorInfo
200214
ret_val &= (working_frame == other.working_frame);
201215
return ret_val;
202216
}
217+
218+
private:
219+
friend class boost::serialization::access;
220+
template <class Archive>
221+
void serialize(Archive& ar, const unsigned int /*version*/)
222+
{
223+
ar& boost::serialization::make_nvp("manipulator", manipulator);
224+
ar& boost::serialization::make_nvp("manipulator_ik_solver", manipulator_ik_solver);
225+
ar& boost::serialization::make_nvp("tcp", tcp);
226+
ar& boost::serialization::make_nvp("working_frame", working_frame);
227+
}
203228
};
204229
} // namespace tesseract_common
205230

‎tesseract_common/include/tesseract_common/serialization.h‎

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
/**
2+
* @file serialization.h
3+
* @brief Additional Boost serialization wrappers
4+
* @details Supports the following
5+
* - Eigen::VectorXd
6+
* - Eigen::Isometry3d
7+
* - Eigen::MatrixX2d
8+
*
9+
* @author Levi Armstrong
10+
* @date February 21, 2021
11+
* @version TODO
12+
* @bug No known bugs
13+
*
14+
* @copyright Copyright (c) 2021, Southwest Research Institute
15+
*
16+
* @par License
17+
* Software License Agreement (Apache License)
18+
* @par
19+
* Licensed under the Apache License, Version 2.0 (the "License");
20+
* you may not use this file except in compliance with the License.
21+
* You may obtain a copy of the License at
22+
* http://www.apache.org/licenses/LICENSE-2.0
23+
* @par
24+
* Unless required by applicable law or agreed to in writing, software
25+
* distributed under the License is distributed on an "AS IS" BASIS,
26+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
27+
* See the License for the specific language governing permissions and
28+
* limitations under the License.
29+
*/
30+
#ifndef TESSERACT_COMMON_SERIALIZATION_H
31+
#define TESSERACT_COMMON_SERIALIZATION_H
32+
33+
#include <tesseract_common/macros.h>
34+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
35+
#include <Eigen/Dense>
36+
#include <boost/serialization/base_object.hpp>
37+
#include <boost/serialization/nvp.hpp>
38+
#include <boost/serialization/array.hpp>
39+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
40+
41+
namespace boost
42+
{
43+
namespace serialization
44+
{
45+
/*****************************/
46+
/****** Eigen::VectorXd ******/
47+
/*****************************/
48+
template <class Archive>
49+
inline void save(Archive& ar, const Eigen::VectorXd& g, const unsigned int /*version*/)
50+
{
51+
long rows = g.rows();
52+
ar& BOOST_SERIALIZATION_NVP(rows);
53+
ar& boost::serialization::make_nvp("data", boost::serialization::make_array(g.data(), rows));
54+
}
55+
56+
template <class Archive>
57+
inline void load(Archive& ar, Eigen::VectorXd& g, const unsigned int /*version*/)
58+
{
59+
long rows;
60+
ar& BOOST_SERIALIZATION_NVP(rows);
61+
g.resize(rows);
62+
ar& boost::serialization::make_nvp("data", boost::serialization::make_array(g.data(), rows));
63+
}
64+
65+
template <class Archive>
66+
inline void serialize(Archive& ar, Eigen::VectorXd& g, const unsigned int version)
67+
{
68+
split_free(ar, g, version);
69+
}
70+
71+
/*****************************/
72+
/****** Eigen::VectorXd ******/
73+
/*****************************/
74+
75+
template <class Archive>
76+
inline void save(Archive& ar, const Eigen::Isometry3d& g, const unsigned int /*version*/)
77+
{
78+
ar& boost::serialization::make_nvp("xyz", boost::serialization::make_array(g.translation().data(), 3));
79+
Eigen::Quaterniond q(g.linear());
80+
ar& boost::serialization::make_nvp("wxyz", boost::serialization::make_array(q.vec().data(), 4));
81+
}
82+
83+
template <class Archive>
84+
inline void load(Archive& ar, Eigen::Isometry3d& g, const unsigned int /*version*/)
85+
{
86+
g.setIdentity();
87+
ar& boost::serialization::make_nvp("xyz", boost::serialization::make_array(g.translation().data(), 3));
88+
Eigen::Quaterniond q;
89+
ar& boost::serialization::make_nvp("wxyz", boost::serialization::make_array(q.vec().data(), 4));
90+
q.normalize();
91+
g.linear() = q.toRotationMatrix();
92+
}
93+
94+
template <class Archive>
95+
inline void serialize(Archive& ar, Eigen::Isometry3d& g, const unsigned int version)
96+
{
97+
split_free(ar, g, version);
98+
}
99+
100+
/*****************************/
101+
/****** Eigen::MatrixX2d *****/
102+
/*****************************/
103+
template <class Archive>
104+
inline void save(Archive& ar, const Eigen::MatrixX2d& g, const unsigned int /*version*/)
105+
{
106+
long rows = g.rows();
107+
ar& BOOST_SERIALIZATION_NVP(rows);
108+
ar& boost::serialization::make_nvp("data", boost::serialization::make_array(g.data(), rows * 2));
109+
}
110+
111+
template <class Archive>
112+
inline void load(Archive& ar, Eigen::MatrixX2d& g, const unsigned int /*version*/)
113+
{
114+
long rows;
115+
ar& BOOST_SERIALIZATION_NVP(rows);
116+
g.resize(rows, 2);
117+
ar& boost::serialization::make_nvp("data", boost::serialization::make_array(g.data(), rows * 2));
118+
}
119+
120+
template <class Archive>
121+
inline void serialize(Archive& ar, Eigen::MatrixX2d& g, const unsigned int version)
122+
{
123+
split_free(ar, g, version);
124+
}
125+
} // namespace serialization
126+
} // namespace boost
127+
128+
#endif // TESSERACT_COMMON_SERIALIZATION_H

‎tesseract_common/include/tesseract_common/types.h‎

Lines changed: 21 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3737
#include <boost/filesystem.hpp>
3838
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3939

40+
#include <tesseract_common/serialization.h>
41+
4042
namespace tesseract_common
4143
{
4244
/** @brief Enable easy switching to std::filesystem when available */
@@ -85,36 +87,6 @@ static inline LinkNamesPair makeOrderedLinkPair(const std::string& link_name1, c
8587
return std::make_pair(link_name2, link_name1);
8688
}
8789

88-
struct JointState
89-
{
90-
JointState() = default;
91-
JointState(std::vector<std::string> joint_names, Eigen::VectorXd position)
92-
: joint_names(std::move(joint_names)), position(std::move(position))
93-
{
94-
}
95-
96-
/** @brief The joint corresponding to the position vector. */
97-
std::vector<std::string> joint_names;
98-
99-
/** @brief The joint position at the waypoint */
100-
Eigen::VectorXd position;
101-
102-
/** @brief The velocity at the waypoint (optional) */
103-
Eigen::VectorXd velocity;
104-
105-
/** @brief The Acceleration at the waypoint (optional) */
106-
Eigen::VectorXd acceleration;
107-
108-
/** @brief The Effort at the waypoint (optional) */
109-
Eigen::VectorXd effort;
110-
111-
/** @brief The Time from start at the waypoint (optional) */
112-
double time{ 0 };
113-
};
114-
115-
/** @brief Represents a joint trajectory */
116-
using JointTrajectory = std::vector<JointState>;
117-
11890
/** @brief Store kinematic limits */
11991
struct KinematicLimits
12092
{
@@ -123,6 +95,25 @@ struct KinematicLimits
12395
Eigen::MatrixX2d joint_limits;
12496
Eigen::VectorXd velocity_limits;
12597
Eigen::VectorXd acceleration_limits;
98+
99+
bool operator==(const KinematicLimits& other) const
100+
{
101+
bool ret_val = true;
102+
ret_val &= (joint_limits.isApprox(other.joint_limits, 1e-5));
103+
ret_val &= (velocity_limits.isApprox(other.velocity_limits, 1e-5));
104+
ret_val &= (acceleration_limits.isApprox(other.acceleration_limits, 1e-5));
105+
return ret_val;
106+
}
107+
108+
private:
109+
friend class boost::serialization::access;
110+
template <class Archive>
111+
void serialize(Archive& ar, const unsigned int /*version*/)
112+
{
113+
ar& BOOST_SERIALIZATION_NVP(joint_limits);
114+
ar& BOOST_SERIALIZATION_NVP(velocity_limits);
115+
ar& BOOST_SERIALIZATION_NVP(acceleration_limits);
116+
}
126117
};
127118
} // namespace tesseract_common
128119
#endif // TESSERACT_COMMON_TYPES_H

0 commit comments

Comments
 (0)