Skip to content

Commit

Permalink
Add boost serialization to profiles
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Dec 6, 2024
1 parent ca1f571 commit 2b16c84
Show file tree
Hide file tree
Showing 47 changed files with 1,051 additions and 661 deletions.
2 changes: 1 addition & 1 deletion tesseract_command_language/include/tesseract_command_language/profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ class Profile
using ConstPtr = std::shared_ptr<const Profile>;

Profile() = default;
Profile(std::size_t id);
Profile(std::size_t key);
Profile(const Profile&) = delete;
Profile& operator=(const Profile&) = delete;
Profile(Profile&&) = delete;
Expand Down
2 changes: 1 addition & 1 deletion tesseract_command_language/include/tesseract_command_language/profile_dictionary.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class ProfileDictionary
* @param profile_name The profile name
* @param profile The profile to add
*/
void addProfile(const std::string& ns, const std::string& profile_name, Profile::ConstPtr profile);
void addProfile(const std::string& ns, const std::string& profile_name, const Profile::ConstPtr& profile);

/**
* @brief Check if a profile exists
Expand Down
303 changes: 151 additions & 152 deletions ...d_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,169 +63,168 @@ void runCartesianWaypointTest()
EXPECT_FALSE(wp.isToleranced());
}

{ // Set/Get Transform
{ // Test construction providing pose
{ // Set/Get Transform
{ // Test construction providing pose
Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T(pose) };
EXPECT_TRUE(wp.getTransform().isApprox(pose));
EXPECT_FALSE(wp.isToleranced());
}
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T(pose) };
EXPECT_TRUE(wp.getTransform().isApprox(pose));
EXPECT_FALSE(wp.isToleranced());
}

{ // Test construction providing pose
Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T(pose, Eigen::VectorXd::Constant(3, -5), Eigen::VectorXd::Constant(3, 5)) };
EXPECT_TRUE(wp.getTransform().isApprox(pose));
EXPECT_TRUE(wp.getLowerTolerance().isApprox(Eigen::VectorXd::Constant(3, -5)));
EXPECT_TRUE(wp.getUpperTolerance().isApprox(Eigen::VectorXd::Constant(3, 5)));
EXPECT_TRUE(wp.isToleranced());
}

{ // Test set pose
Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T() };
wp.setTransform(pose);
EXPECT_TRUE(wp.getTransform().isApprox(pose));
EXPECT_FALSE(wp.isToleranced());
}

{ // Test assigning pose
Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T() };
wp.getTransform() = pose;
EXPECT_TRUE(std::as_const(wp).getTransform().isApprox(pose));
EXPECT_FALSE(wp.isToleranced());
}
} // namespace tesseract_planning::test_suite

{ // Test Set Tolerances
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.isToleranced());
{ // Test construction providing pose
Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T(pose, Eigen::VectorXd::Constant(3, -5), Eigen::VectorXd::Constant(3, 5)) };
EXPECT_TRUE(wp.getTransform().isApprox(pose));
EXPECT_TRUE(wp.getLowerTolerance().isApprox(Eigen::VectorXd::Constant(3, -5)));
EXPECT_TRUE(wp.getUpperTolerance().isApprox(Eigen::VectorXd::Constant(3, 5)));
EXPECT_TRUE(wp.isToleranced());
}

wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5));
EXPECT_TRUE(wp.isToleranced());
{ // Test set pose
Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T() };
wp.setTransform(pose);
EXPECT_TRUE(wp.getTransform().isApprox(pose));
EXPECT_FALSE(wp.isToleranced());
}

wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5));
EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT
{ // Test assigning pose
Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
pose.translation() = Eigen::Vector3d(1, 2, 3);
CartesianWaypointPoly wp{ T() };
wp.getTransform() = pose;
EXPECT_TRUE(std::as_const(wp).getTransform().isApprox(pose));
EXPECT_FALSE(wp.isToleranced());
}
} // namespace tesseract_planning::test_suite

wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5));
EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT
{ // Test Set Tolerances
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.isToleranced());

wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0));
EXPECT_FALSE(wp.isToleranced());
wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5));
EXPECT_TRUE(wp.isToleranced());

wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5));
EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT

wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5));
EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT

wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0));
wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0));
EXPECT_FALSE(wp.isToleranced());
}

{ // Test Equality and Serialization
{ CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2(wp1); // NOLINT
EXPECT_TRUE(wp1 == wp2);
EXPECT_TRUE(wp2 == wp1);
EXPECT_FALSE(wp2 != wp1);
runWaypointSerializationTest(wp1);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
wp1.getTransform().translate(Eigen::Vector3d(1e6, 0, 0));
CartesianWaypointPoly wp2(wp1);
EXPECT_TRUE(wp1 == wp2);
EXPECT_TRUE(wp2 == wp1);
EXPECT_FALSE(wp2 != wp1);
runWaypointSerializationTest(wp1);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
wp1.getUpperTolerance().resize(3);
wp1.getUpperTolerance() << 1, 2, 3;
wp1.getLowerTolerance().resize(3);
wp1.getLowerTolerance() << -4, -5, -6;
CartesianWaypointPoly wp2(wp1);
EXPECT_TRUE(wp1 == wp2);
EXPECT_TRUE(wp2 == wp1);
EXPECT_FALSE(wp2 != wp1);
runWaypointSerializationTest(wp1);
}
// Not equal
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) };
wp2.getTransform().rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1)));
EXPECT_FALSE(wp1 == wp2);
EXPECT_FALSE(wp2 == wp1);
EXPECT_TRUE(wp2 != wp1);
runWaypointSerializationTest(wp2);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) };
wp2.getTransform().translate(Eigen::Vector3d(1e6, 0, 0));
EXPECT_FALSE(wp1 == wp2);
EXPECT_FALSE(wp2 == wp1);
EXPECT_TRUE(wp2 != wp1);
runWaypointSerializationTest(wp2);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2(wp1);
wp2.getUpperTolerance().resize(3);
wp2.getUpperTolerance() << 1, 2, 3;
wp2.getLowerTolerance().resize(3);
wp2.getLowerTolerance() << -4, -5, -6;
EXPECT_FALSE(wp1 == wp2);
EXPECT_FALSE(wp2 == wp1);
EXPECT_TRUE(wp2 != wp1);
runWaypointSerializationTest(wp2);
}
}

{ // Set/Get Seed
tesseract_common::JointState seed_state;
seed_state.joint_names = { "joint_1", "joint_2", "joint_3" };
seed_state.position.resize(3);
seed_state.position << .01, .02, .03;
seed_state.velocity.resize(3);
seed_state.velocity << .1, .2, .3;
seed_state.acceleration.resize(3);
seed_state.acceleration << 1, 2, 3;

{ // Test default construction pose
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
EXPECT_FALSE(std::as_const(wp).getSeed() == seed_state);
}

{ // Test Equality and Serialization
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2(wp1); // NOLINT
EXPECT_TRUE(wp1 == wp2);
EXPECT_TRUE(wp2 == wp1);
EXPECT_FALSE(wp2 != wp1);
runWaypointSerializationTest(wp1);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
wp1.getTransform().translate(Eigen::Vector3d(1e6, 0, 0));
CartesianWaypointPoly wp2(wp1);
EXPECT_TRUE(wp1 == wp2);
EXPECT_TRUE(wp2 == wp1);
EXPECT_FALSE(wp2 != wp1);
runWaypointSerializationTest(wp1);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
wp1.getUpperTolerance().resize(3);
wp1.getUpperTolerance() << 1, 2, 3;
wp1.getLowerTolerance().resize(3);
wp1.getLowerTolerance() << -4, -5, -6;
CartesianWaypointPoly wp2(wp1);
EXPECT_TRUE(wp1 == wp2);
EXPECT_TRUE(wp2 == wp1);
EXPECT_FALSE(wp2 != wp1);
runWaypointSerializationTest(wp1);
}
// Not equal
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) };
wp2.getTransform().rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1)));
EXPECT_FALSE(wp1 == wp2);
EXPECT_FALSE(wp2 == wp1);
EXPECT_TRUE(wp2 != wp1);
runWaypointSerializationTest(wp2);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) };
wp2.getTransform().translate(Eigen::Vector3d(1e6, 0, 0));
EXPECT_FALSE(wp1 == wp2);
EXPECT_FALSE(wp2 == wp1);
EXPECT_TRUE(wp2 != wp1);
runWaypointSerializationTest(wp2);
}
{
CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) };
CartesianWaypointPoly wp2(wp1);
wp2.getUpperTolerance().resize(3);
wp2.getUpperTolerance() << 1, 2, 3;
wp2.getLowerTolerance().resize(3);
wp2.getLowerTolerance() << -4, -5, -6;
EXPECT_FALSE(wp1 == wp2);
EXPECT_FALSE(wp2 == wp1);
EXPECT_TRUE(wp2 != wp1);
runWaypointSerializationTest(wp2);
}
{ // Test assigning pose
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
wp.setSeed(seed_state);
EXPECT_TRUE(wp.hasSeed());
EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state);
}

{ // Test assigning pose
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
wp.getSeed() = seed_state;
EXPECT_TRUE(wp.hasSeed());
EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state);
}

{ // Set/Get Seed
tesseract_common::JointState seed_state;
seed_state.joint_names = { "joint_1", "joint_2", "joint_3" };
seed_state.position.resize(3);
seed_state.position << .01, .02, .03;
seed_state.velocity.resize(3);
seed_state.velocity << .1, .2, .3;
seed_state.acceleration.resize(3);
seed_state.acceleration << 1, 2, 3;

{ // Test default construction pose
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
EXPECT_FALSE(std::as_const(wp).getSeed() == seed_state);
}

{ // Test assigning pose
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
wp.setSeed(seed_state);
EXPECT_TRUE(wp.hasSeed());
EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state);
}

{ // Test assigning pose
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
wp.getSeed() = seed_state;
EXPECT_TRUE(wp.hasSeed());
EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state);
}

{ // Test clear seed
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
wp.getSeed() = seed_state;
EXPECT_TRUE(wp.hasSeed());
wp.clearSeed();
EXPECT_FALSE(wp.hasSeed());
}
{ // Test clear seed
CartesianWaypointPoly wp{ T() };
EXPECT_FALSE(wp.hasSeed());
wp.getSeed() = seed_state;
EXPECT_TRUE(wp.hasSeed());
wp.clearSeed();
EXPECT_FALSE(wp.hasSeed());
}
}
}
} // namespace tesseract_planning::test_suite
#endif // TESSERACT_COMMAND_LANGUAGE_CARTESIAN_WAYPOINT_POLY_UNIT_HPP
Loading

0 comments on commit 2b16c84

Please sign in to comment.