diff --git a/tesseract_command_language/include/tesseract_command_language/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h index 6d73f9de74..2a97d726d2 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile.h +++ b/tesseract_command_language/include/tesseract_command_language/profile.h @@ -42,7 +42,7 @@ class Profile using ConstPtr = std::shared_ptr; Profile() = default; - Profile(std::size_t id); + Profile(std::size_t key); Profile(const Profile&) = delete; Profile& operator=(const Profile&) = delete; Profile(Profile&&) = delete; diff --git a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h index d22194faa7..f3e58c4ba5 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -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 diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp index e599918da6..73e0be72e7 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp @@ -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 diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp index ca119e4d08..5353ab1545 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp @@ -52,228 +52,227 @@ void runJointWaypointTest() EXPECT_FALSE(base.isStateWaypoint()); } - { // Test construction - { - JointWaypointPoly wp{ T() }; - EXPECT_TRUE(wp.getNames().empty()); - EXPECT_TRUE(wp.getPosition().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); - EXPECT_FALSE(wp.isConstrained()); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - JointWaypointPoly wp{ T(names, positions) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); - EXPECT_TRUE(wp.isConstrained()); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); - JointWaypointPoly wp{ T(names, positions, lower_tol, uppert_tol) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getLowerTolerance().isApprox(lower_tol)); - EXPECT_TRUE(wp.getUpperTolerance().isApprox(uppert_tol)); - EXPECT_TRUE(wp.isConstrained()); - } - { - std::vector names{ "j1", "j2" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(2, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(2, 5); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT - } - } // namespace tesseract_planning::test_suite + { // Test construction + { JointWaypointPoly wp{ T() }; + EXPECT_TRUE(wp.getNames().empty()); + EXPECT_TRUE(wp.getPosition().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); + EXPECT_FALSE(wp.isConstrained()); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + JointWaypointPoly wp{ T(names, positions) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); + EXPECT_TRUE(wp.isConstrained()); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); + JointWaypointPoly wp{ T(names, positions, lower_tol, uppert_tol) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getLowerTolerance().isApprox(lower_tol)); + EXPECT_TRUE(wp.getUpperTolerance().isApprox(uppert_tol)); + EXPECT_TRUE(wp.isConstrained()); +} +{ + std::vector names{ "j1", "j2" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(2, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(2, 5); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT +} +} // namespace tesseract_planning::test_suite - { // Test is constrained +{ // Test is constrained + JointWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.isConstrained()); + wp.setIsConstrained(true); + EXPECT_TRUE(wp.isConstrained()); +} + +{ // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set JointWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isConstrained()); - wp.setIsConstrained(true); - EXPECT_TRUE(wp.isConstrained()); + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); } - { // Set/Get Names - const std::vector names{ "j1", "j2", "j3" }; - { // Test set - JointWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); - } - - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); - } + { // Test assigning + JointWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); } +} - { // Set/Get Positions - Eigen::VectorXd positions; - positions.resize(3); - positions << 1.0, 2.0, 3.0; - - { // Test set - JointWaypointPoly wp{ T() }; - wp.setPosition(positions); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - } +{ // Set/Get Positions + Eigen::VectorXd positions; + positions.resize(3); + positions << 1.0, 2.0, 3.0; - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getPosition() = positions; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); - } + { // Test set + JointWaypointPoly wp{ T() }; + wp.setPosition(positions); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); } - { // Test Set Tolerances + { // Test assigning JointWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isToleranced()); + wp.getPosition() = positions; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + } +} - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_TRUE(wp.isToleranced()); +{ // Test Set Tolerances + JointWaypointPoly wp{ T() }; + EXPECT_FALSE(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_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()); - } + 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 - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; - const JointWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; - JointWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, upper_tol, lower_tol; - positions.resize(3); - upper_tol.resize(3); - lower_tol.resize(3); - positions << 0, -1e6, 1e6; - upper_tol << 1, 2, 3; - lower_tol << -4, -5, -6; +{ // Test Equality and Serialization + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; + const JointWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; + JointWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, upper_tol, lower_tol; + positions.resize(3); + upper_tol.resize(3); + lower_tol.resize(3); + positions << 0, -1e6, 1e6; + upper_tol << 1, 2, 3; + lower_tol << -4, -5, -6; - JointWaypointPoly wp1{ T(names, positions, lower_tol, upper_tol) }; - JointWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - // Not equal - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T({ "j1", "j2", "j3" }, { 0.001, 0, 0 }) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }, { 1, 2, 3 }, { -4, -5, -6 }) }; - JointWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - Eigen::VectorXd upper_tol, lower_tol; - upper_tol.resize(3); - lower_tol.resize(3); - upper_tol << 1, 2, 3; - lower_tol << -4, -5, -6; - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2(wp1); - wp1.getUpperTolerance() = upper_tol; - wp1.getLowerTolerance() = lower_tol; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } + JointWaypointPoly wp1{ T(names, positions, lower_tol, upper_tol) }; + JointWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + // Not equal + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T({ "j1", "j2", "j3" }, { 0.001, 0, 0 }) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }, { 1, 2, 3 }, { -4, -5, -6 }) }; + JointWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + Eigen::VectorXd upper_tol, lower_tol; + upper_tol.resize(3); + lower_tol.resize(3); + upper_tol << 1, 2, 3; + lower_tol << -4, -5, -6; + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2(wp1); + wp1.getUpperTolerance() = upper_tol; + wp1.getLowerTolerance() = lower_tol; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } +} } } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_JOINT_WAYPOINT_POLY_UNIT_HPP diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp index 991a77d5c5..08a14b851a 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp @@ -52,257 +52,256 @@ void runStateWaypointTest() EXPECT_TRUE(base.isStateWaypoint()); } - { // Test construction - { - StateWaypointPoly wp{ T() }; - EXPECT_TRUE(wp.getNames().empty()); - EXPECT_TRUE(wp.getPosition().rows() == 0); - EXPECT_TRUE(wp.getVelocity().rows() == 0); - EXPECT_TRUE(wp.getAcceleration().rows() == 0); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - StateWaypointPoly wp{ T(names, positions) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getVelocity().rows() == 0); - EXPECT_TRUE(wp.getAcceleration().rows() == 0); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); - StateWaypointPoly wp{ T(names, positions, velocities, accelerations, 5) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getVelocity().isApprox(velocities)); - EXPECT_TRUE(wp.getAcceleration().isApprox(accelerations)); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 5)); - } - { - std::vector names{ "j1", "j2" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(2, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(2, 5); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT - } - } // namespace tesseract_planning::test_suite - - { // Set/Get Names - const std::vector names{ "j1", "j2", "j3" }; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); - } + { // Test construction + { StateWaypointPoly wp{ T() }; + EXPECT_TRUE(wp.getNames().empty()); + EXPECT_TRUE(wp.getPosition().rows() == 0); + EXPECT_TRUE(wp.getVelocity().rows() == 0); + EXPECT_TRUE(wp.getAcceleration().rows() == 0); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + StateWaypointPoly wp{ T(names, positions) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getVelocity().rows() == 0); + EXPECT_TRUE(wp.getAcceleration().rows() == 0); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); + StateWaypointPoly wp{ T(names, positions, velocities, accelerations, 5) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getVelocity().isApprox(velocities)); + EXPECT_TRUE(wp.getAcceleration().isApprox(accelerations)); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 5)); +} +{ + std::vector names{ "j1", "j2" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(2, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(2, 5); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT +} +} // namespace tesseract_planning::test_suite - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); - } +{ // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set + StateWaypointPoly wp{ T() }; + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); } - { // Set/Get Positions - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setPosition(data); - EXPECT_TRUE(wp.getPosition().isApprox(data)); - } +{ // Set/Get Positions + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getPosition() = data; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setPosition(data); + EXPECT_TRUE(wp.getPosition().isApprox(data)); } - { // Set/Get Velocity - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getPosition() = data; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setVelocity(data); - EXPECT_TRUE(wp.getVelocity().isApprox(data)); - } +{ // Set/Get Velocity + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getVelocity() = data; - EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setVelocity(data); + EXPECT_TRUE(wp.getVelocity().isApprox(data)); } - { // Set/Get Acceleration - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getVelocity() = data; + EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setAcceleration(data); - EXPECT_TRUE(wp.getAcceleration().isApprox(data)); - } +{ // Set/Get Acceleration + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getAcceleration() = data; - EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setAcceleration(data); + EXPECT_TRUE(wp.getAcceleration().isApprox(data)); } - { // Set/Get Effort - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getAcceleration() = data; + EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setEffort(data); - EXPECT_TRUE(wp.getEffort().isApprox(data)); - } +{ // Set/Get Effort + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getEffort() = data; - EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setEffort(data); + EXPECT_TRUE(wp.getEffort().isApprox(data)); } - { // Set/Get Time - double data{ 3 }; + { // Test assigning StateWaypointPoly wp{ T() }; - wp.setTime(data); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), data)); + wp.getEffort() = data; + EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); } +} + +{ // Set/Get Time + double data{ 3 }; + StateWaypointPoly wp{ T() }; + wp.setTime(data); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), data)); +} - { // Test Equality and Serialization - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; - const StateWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; - StateWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, velocities, accelerations, efforts; - positions.resize(3); - velocities.resize(3); - accelerations.resize(3); - efforts.resize(3); - positions << 0, -1e6, 1e6; - velocities << 1, 2, 3; - accelerations << -4, -5, -6; - efforts << 4, 5, 6; +{ // Test Equality and Serialization + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; + const StateWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; + StateWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, velocities, accelerations, efforts; + positions.resize(3); + velocities.resize(3); + accelerations.resize(3); + efforts.resize(3); + positions << 0, -1e6, 1e6; + velocities << 1, 2, 3; + accelerations << -4, -5, -6; + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T(names, positions, velocities, accelerations, 5) }; - wp1.getEffort() = efforts; - StateWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - Eigen::VectorXd efforts; - efforts.resize(3); - efforts << 4, 5, 6; + StateWaypointPoly wp1{ T(names, positions, velocities, accelerations, 5) }; + wp1.getEffort() = efforts; + StateWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + Eigen::VectorXd efforts; + efforts.resize(3); + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }, { 1, 2, 3 }, { -4, -5, -6 }, 5) }; - wp1.getEffort() = efforts; - StateWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - // Not equal - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, velocities, accelerations, efforts; - positions.resize(3); - velocities.resize(3); - accelerations.resize(3); - efforts.resize(3); - positions << 0, -1e6, 1e6; - velocities << 1, 2, 3; - accelerations << -4, -5, -6; - efforts << 4, 5, 6; + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }, { 1, 2, 3 }, { -4, -5, -6 }, 5) }; + wp1.getEffort() = efforts; + StateWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + // Not equal + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, velocities, accelerations, efforts; + positions.resize(3); + velocities.resize(3); + accelerations.resize(3); + efforts.resize(3); + positions << 0, -1e6, 1e6; + velocities << 1, 2, 3; + accelerations << -4, -5, -6; + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ wp1 }; - wp1.getNames() = names; - wp1.getPosition() = positions; - wp1.getVelocity() = velocities; - wp1.getAcceleration() = accelerations; - wp1.getEffort() = efforts; - wp1.setTime(5); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ wp1 }; + wp1.getNames() = names; + wp1.getPosition() = positions; + wp1.getVelocity() = velocities; + wp1.getAcceleration() = accelerations; + wp1.getEffort() = efforts; + wp1.setTime(5); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); } } +} } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_STATE_WAYPOINT_POLY_UNIT_HPP diff --git a/tesseract_command_language/src/profile_dictionary.cpp b/tesseract_command_language/src/profile_dictionary.cpp index e788657905..30f3ea26b5 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -68,7 +68,9 @@ std::unordered_map ProfileDictionary::getProfile ns + "'!"); } -void ProfileDictionary::addProfile(const std::string& ns, const std::string& profile_name, Profile::ConstPtr profile) +void ProfileDictionary::addProfile(const std::string& ns, + const std::string& profile_name, + const Profile::ConstPtr& profile) { if (ns.empty()) throw std::runtime_error("Adding profile with an empty namespace!"); diff --git a/tesseract_motion_planners/core/CMakeLists.txt b/tesseract_motion_planners/core/CMakeLists.txt index 949263a39a..1ee03a59fe 100644 --- a/tesseract_motion_planners/core/CMakeLists.txt +++ b/tesseract_motion_planners/core/CMakeLists.txt @@ -1,8 +1,5 @@ # Create interface for core -add_library(${PROJECT_NAME}_core - src/planner.cpp - src/types.cpp - src/utils.cpp) +add_library(${PROJECT_NAME}_core src/planner.cpp src/types.cpp src/utils.cpp) target_link_libraries( ${PROJECT_NAME}_core PUBLIC tesseract::tesseract_environment diff --git a/tesseract_motion_planners/descartes/CMakeLists.txt b/tesseract_motion_planners/descartes/CMakeLists.txt index 097b694dcf..5eafd22e6a 100644 --- a/tesseract_motion_planners/descartes/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/CMakeLists.txt @@ -12,8 +12,7 @@ add_library( src/deserialize.cpp src/descartes_utils.cpp src/profile/descartes_profile.cpp - src/profile/descartes_default_plan_profile.cpp -) + src/profile/descartes_default_plan_profile.cpp) target_link_libraries( ${PROJECT_NAME}_descartes PUBLIC ${PROJECT_NAME}_core diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h index a52bdc9eba..1bdde8ee7d 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h @@ -47,11 +47,6 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile using ConstPtr = std::shared_ptr>; DescartesDefaultPlanProfile() = default; - ~DescartesDefaultPlanProfile() override = default; - DescartesDefaultPlanProfile(const DescartesDefaultPlanProfile&) = default; - DescartesDefaultPlanProfile& operator=(const DescartesDefaultPlanProfile&) = default; - DescartesDefaultPlanProfile(DescartesDefaultPlanProfile&&) noexcept = default; - DescartesDefaultPlanProfile& operator=(DescartesDefaultPlanProfile&&) noexcept = default; DescartesDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); PoseSamplerFn target_pose_sampler = sampleFixed; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h index b880b75f8d..8f90e755ff 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h @@ -54,10 +54,6 @@ class DescartesPlanProfile : public Profile using ConstPtr = std::shared_ptr>; DescartesPlanProfile() : Profile(DescartesPlanProfile::getStaticKey()) {} - DescartesPlanProfile(const DescartesPlanProfile&) = delete; - DescartesPlanProfile& operator=(const DescartesPlanProfile&) = delete; - DescartesPlanProfile(DescartesPlanProfile&&) noexcept = delete; - DescartesPlanProfile& operator=(DescartesPlanProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp index f73cc5d87d..70d073c23b 100644 --- a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp +++ b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp @@ -24,8 +24,10 @@ * limitations under the License. */ #include +#include #include #include +#include namespace tesseract_planning { @@ -38,6 +40,19 @@ template void DescartesDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DescartesPlanProfile); + /** @todo FIX */ + // ar& BOOST_SERIALIZATION_NVP(target_pose_sampler); + // ar& BOOST_SERIALIZATION_NVP(edge_evaluator); + // ar& BOOST_SERIALIZATION_NVP(state_evaluator); + // ar& BOOST_SERIALIZATION_NVP(vertex_evaluator); + ar& BOOST_SERIALIZATION_NVP(allow_collision); + ar& BOOST_SERIALIZATION_NVP(enable_collision); + ar& BOOST_SERIALIZATION_NVP(vertex_collision_check_config); + ar& BOOST_SERIALIZATION_NVP(enable_edge_collision); + ar& BOOST_SERIALIZATION_NVP(edge_collision_check_config); + ar& BOOST_SERIALIZATION_NVP(use_redundant_joint_solutions); + ar& BOOST_SERIALIZATION_NVP(num_threads); + ar& BOOST_SERIALIZATION_NVP(debug); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h index 87cf2a1421..c4f3dcfe87 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h @@ -35,6 +35,8 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tinyxml2 @@ -87,6 +89,11 @@ struct OMPLPlannerConfigurator virtual OMPLPlannerType getType() const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct SBLConfigurator : public OMPLPlannerConfigurator @@ -109,6 +116,11 @@ struct SBLConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct ESTConfigurator : public OMPLPlannerConfigurator @@ -134,6 +146,11 @@ struct ESTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct LBKPIECE1Configurator : public OMPLPlannerConfigurator @@ -162,6 +179,11 @@ struct LBKPIECE1Configurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct BKPIECE1Configurator : public OMPLPlannerConfigurator @@ -193,6 +215,11 @@ struct BKPIECE1Configurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct KPIECE1Configurator : public OMPLPlannerConfigurator @@ -227,6 +254,11 @@ struct KPIECE1Configurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct BiTRRTConfigurator : public OMPLPlannerConfigurator @@ -265,6 +297,11 @@ struct BiTRRTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct RRTConfigurator : public OMPLPlannerConfigurator @@ -290,6 +327,11 @@ struct RRTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct RRTConnectConfigurator : public OMPLPlannerConfigurator @@ -312,6 +354,11 @@ struct RRTConnectConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct RRTstarConfigurator : public OMPLPlannerConfigurator @@ -340,6 +387,11 @@ struct RRTstarConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct TRRTConfigurator : public OMPLPlannerConfigurator @@ -377,6 +429,11 @@ struct TRRTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct PRMConfigurator : public OMPLPlannerConfigurator @@ -399,6 +456,11 @@ struct PRMConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct PRMstarConfigurator : public OMPLPlannerConfigurator @@ -418,6 +480,11 @@ struct PRMstarConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct LazyPRMstarConfigurator : public OMPLPlannerConfigurator @@ -437,6 +504,11 @@ struct LazyPRMstarConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct SPARSConfigurator : public OMPLPlannerConfigurator @@ -468,8 +540,29 @@ struct SPARSConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLPlannerConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SBLConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::ESTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::LBKPIECE1Configurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::BKPIECE1Configurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::KPIECE1Configurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::BiTRRTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RRTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RRTConnectConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RRTstarConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TRRTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::PRMConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::PRMstarConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::LazyPRMstarConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SPARSConfigurator) + #endif // TESSERACT_MOTION_PLANNERS_OMPL_PLANNER_CONFIGURATOR_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h index fbb7e9f4c7..54d3469cf8 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h @@ -62,11 +62,6 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile using ConstPtr = std::shared_ptr; OMPLDefaultPlanProfile(); - ~OMPLDefaultPlanProfile() override = default; - OMPLDefaultPlanProfile(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile& operator=(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile(OMPLDefaultPlanProfile&&) noexcept = default; - OMPLDefaultPlanProfile& operator=(OMPLDefaultPlanProfile&&) noexcept = default; OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); /** @brief The OMPL state space to use when planning */ diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h index f02e779f54..1d5822238a 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h @@ -53,10 +53,6 @@ class OMPLPlanProfile : public Profile using ConstPtr = std::shared_ptr; OMPLPlanProfile(); - OMPLPlanProfile(const OMPLPlanProfile&) = delete; - OMPLPlanProfile& operator=(const OMPLPlanProfile&) = delete; - OMPLPlanProfile(OMPLPlanProfile&&) noexcept = delete; - OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID diff --git a/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp b/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp index d23c36466d..9488d20ce2 100644 --- a/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp @@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -52,6 +53,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { +template +void OMPLPlannerConfigurator::serialize(Archive& /*ar*/, const unsigned int /*version*/) +{ +} + SBLConfigurator::SBLConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* sbl_element = xml_element.FirstChildElement("SBL"); @@ -91,6 +97,13 @@ tinyxml2::XMLElement* SBLConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void SBLConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); +} + ESTConfigurator::ESTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* est_element = xml_element.FirstChildElement("EST"); @@ -151,6 +164,14 @@ tinyxml2::XMLElement* ESTConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void ESTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); +} + LBKPIECE1Configurator::LBKPIECE1Configurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* lbkpiece1_element = xml_element.FirstChildElement("LBKPIECE1"); @@ -231,6 +252,15 @@ tinyxml2::XMLElement* LBKPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) c return ompl_xml; } +template +void LBKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(border_fraction); + ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); +} + BKPIECE1Configurator::BKPIECE1Configurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* bkpiece1_element = xml_element.FirstChildElement("BKPIECE1"); @@ -333,6 +363,16 @@ tinyxml2::XMLElement* BKPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) co return ompl_xml; } +template +void BKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(border_fraction); + ar& BOOST_SERIALIZATION_NVP(failed_expansion_score_factor); + ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); +} + KPIECE1Configurator::KPIECE1Configurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* kpiece1_element = xml_element.FirstChildElement("KPIECE1"); @@ -454,6 +494,17 @@ tinyxml2::XMLElement* KPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) con return ompl_xml; } +template +void KPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); + ar& BOOST_SERIALIZATION_NVP(border_fraction); + ar& BOOST_SERIALIZATION_NVP(failed_expansion_score_factor); + ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); +} + BiTRRTConfigurator::BiTRRTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* bitrrt_element = xml_element.FirstChildElement("BiTRRT"); @@ -593,6 +644,18 @@ tinyxml2::XMLElement* BiTRRTConfigurator::toXML(tinyxml2::XMLDocument& doc) cons return ompl_xml; } +template +void BiTRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(temp_change_factor); + ar& BOOST_SERIALIZATION_NVP(cost_threshold); + ar& BOOST_SERIALIZATION_NVP(init_temperature); + ar& BOOST_SERIALIZATION_NVP(frontier_threshold); + ar& BOOST_SERIALIZATION_NVP(frontier_node_ratio); +} + RRTConfigurator::RRTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* rrt_element = xml_element.FirstChildElement("RRT"); @@ -653,6 +716,14 @@ tinyxml2::XMLElement* RRTConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void RRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); +} + RRTConnectConfigurator::RRTConnectConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* rrt_connect_element = xml_element.FirstChildElement("RRTConnect"); @@ -694,6 +765,13 @@ tinyxml2::XMLElement* RRTConnectConfigurator::toXML(tinyxml2::XMLDocument& doc) return ompl_xml; } +template +void RRTConnectConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); +} + RRTstarConfigurator::RRTstarConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* rrt_star_element = xml_element.FirstChildElement("RRTstar"); @@ -769,6 +847,15 @@ tinyxml2::XMLElement* RRTstarConfigurator::toXML(tinyxml2::XMLDocument& doc) con return ompl_xml; } +template +void RRTstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); + ar& BOOST_SERIALIZATION_NVP(delay_collision_checking); +} + TRRTConfigurator::TRRTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* trrt_element = xml_element.FirstChildElement("TRRT"); @@ -905,6 +992,18 @@ tinyxml2::XMLElement* TRRTConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void TRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); + ar& BOOST_SERIALIZATION_NVP(temp_change_factor); + ar& BOOST_SERIALIZATION_NVP(init_temperature); + ar& BOOST_SERIALIZATION_NVP(frontier_threshold); + ar& BOOST_SERIALIZATION_NVP(frontier_node_ratio); +} + PRMConfigurator::PRMConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* prm_element = xml_element.FirstChildElement("PRM"); @@ -946,6 +1045,13 @@ tinyxml2::XMLElement* PRMConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void PRMConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(max_nearest_neighbors); +} + PRMstarConfigurator::PRMstarConfigurator(const tinyxml2::XMLElement&) {} ompl::base::PlannerPtr PRMstarConfigurator::create(ompl::base::SpaceInformationPtr si) const @@ -962,6 +1068,12 @@ tinyxml2::XMLElement* PRMstarConfigurator::toXML(tinyxml2::XMLDocument& doc) con return ompl_xml; } +template +void PRMstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); +} + LazyPRMstarConfigurator::LazyPRMstarConfigurator(const tinyxml2::XMLElement&) {} ompl::base::PlannerPtr LazyPRMstarConfigurator::create(ompl::base::SpaceInformationPtr si) const @@ -978,6 +1090,12 @@ tinyxml2::XMLElement* LazyPRMstarConfigurator::toXML(tinyxml2::XMLDocument& doc) return ompl_xml; } +template +void LazyPRMstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); +} + SPARSConfigurator::SPARSConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* spars_element = xml_element.FirstChildElement("SPARS"); @@ -1075,4 +1193,47 @@ tinyxml2::XMLElement* SPARSConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } + +template +void SPARSConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(max_failures); + ar& BOOST_SERIALIZATION_NVP(dense_delta_fraction); + ar& BOOST_SERIALIZATION_NVP(sparse_delta_fraction); + ar& BOOST_SERIALIZATION_NVP(stretch_factor); +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLPlannerConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SBLConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ESTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::LBKPIECE1Configurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::BKPIECE1Configurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::KPIECE1Configurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::BiTRRTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RRTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RRTConnectConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RRTstarConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TRRTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::PRMConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::PRMstarConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::LazyPRMstarConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SPARSConfigurator) + +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLPlannerConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SBLConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ESTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::LBKPIECE1Configurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::BKPIECE1Configurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::KPIECE1Configurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::BiTRRTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RRTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RRTConnectConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RRTstarConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TRRTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::PRMConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::PRMstarConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::LazyPRMstarConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SPARSConfigurator) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp index 08231b7275..e8a29f3619 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp @@ -35,6 +35,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -52,7 +54,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include +#include namespace tesseract_planning { @@ -783,6 +787,17 @@ template void OMPLDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_space); + ar& BOOST_SERIALIZATION_NVP(planning_time); + ar& BOOST_SERIALIZATION_NVP(max_solutions); + ar& BOOST_SERIALIZATION_NVP(simplify); + ar& BOOST_SERIALIZATION_NVP(optimize); + ar& BOOST_SERIALIZATION_NVP(planners); + ar& BOOST_SERIALIZATION_NVP(collision_check_config); + // ar& BOOST_SERIALIZATION_NVP(state_sampler_allocator); + // ar& BOOST_SERIALIZATION_NVP(optimization_objective_allocator); + // ar& BOOST_SERIALIZATION_NVP(svc_allocator); + // ar& BOOST_SERIALIZATION_NVP(mv_allocator); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h index fe4ff85973..05dd83a33e 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h @@ -50,10 +50,6 @@ class SimplePlannerPlanProfile : public Profile using ConstPtr = std::shared_ptr; SimplePlannerPlanProfile(); - SimplePlannerPlanProfile(const SimplePlannerPlanProfile&) = delete; - SimplePlannerPlanProfile& operator=(const SimplePlannerPlanProfile&) = delete; - SimplePlannerPlanProfile(SimplePlannerPlanProfile&&) noexcept = delete; - SimplePlannerPlanProfile& operator=(SimplePlannerPlanProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID @@ -93,10 +89,6 @@ class SimplePlannerCompositeProfile : public Profile using ConstPtr = std::shared_ptr; SimplePlannerCompositeProfile(); - SimplePlannerCompositeProfile(const SimplePlannerCompositeProfile&) = delete; - SimplePlannerCompositeProfile& operator=(const SimplePlannerCompositeProfile&) = delete; - SimplePlannerCompositeProfile(SimplePlannerCompositeProfile&&) noexcept = delete; - SimplePlannerCompositeProfile& operator=(SimplePlannerCompositeProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp index 3356d09a84..9bf5c2e475 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp @@ -131,6 +131,8 @@ template void SimplePlannerFixedSizeAssignPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(freespace_steps); + ar& BOOST_SERIALIZATION_NVP(linear_steps); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp index 11c83b703e..b6efdedb9b 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp @@ -70,6 +70,8 @@ template void SimplePlannerFixedSizePlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(freespace_steps); + ar& BOOST_SERIALIZATION_NVP(linear_steps); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp index be4a21576c..5efa23ecb4 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp @@ -103,6 +103,11 @@ template void SimplePlannerLVSNoIKPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(translation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(rotation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(min_steps); + ar& BOOST_SERIALIZATION_NVP(max_steps); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp index c38a28264f..fc54a00112 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp @@ -103,6 +103,11 @@ template void SimplePlannerLVSPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(translation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(rotation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(min_steps); + ar& BOOST_SERIALIZATION_NVP(max_steps); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h index a45debb90f..1058890e86 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h @@ -51,12 +51,7 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile using ConstPtr = std::shared_ptr; TrajOptDefaultCompositeProfile() = default; - ~TrajOptDefaultCompositeProfile() override = default; TrajOptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element); - TrajOptDefaultCompositeProfile(const TrajOptDefaultCompositeProfile&) = default; - TrajOptDefaultCompositeProfile& operator=(const TrajOptDefaultCompositeProfile&) = default; - TrajOptDefaultCompositeProfile(TrajOptDefaultCompositeProfile&&) = default; - TrajOptDefaultCompositeProfile& operator=(TrajOptDefaultCompositeProfile&&) = default; /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */ tesseract_collision::ContactTestType contact_test_type{ tesseract_collision::ContactTestType::ALL }; diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h index 7da66147c0..affec26a22 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h @@ -45,12 +45,7 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile using ConstPtr = std::shared_ptr; TrajOptDefaultPlanProfile() = default; - ~TrajOptDefaultPlanProfile() override = default; TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); - TrajOptDefaultPlanProfile(const TrajOptDefaultPlanProfile&) = default; - TrajOptDefaultPlanProfile& operator=(const TrajOptDefaultPlanProfile&) = default; - TrajOptDefaultPlanProfile(TrajOptDefaultPlanProfile&&) = default; - TrajOptDefaultPlanProfile& operator=(TrajOptDefaultPlanProfile&&) = default; CartesianWaypointConfig cartesian_cost_config; CartesianWaypointConfig cartesian_constraint_config; diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h index 908911a136..c69e3add39 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h @@ -38,13 +38,6 @@ class TrajOptDefaultSolverProfile : public TrajOptSolverProfile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptDefaultSolverProfile() = default; - ~TrajOptDefaultSolverProfile() override = default; - TrajOptDefaultSolverProfile(const TrajOptDefaultSolverProfile&) = default; - TrajOptDefaultSolverProfile& operator=(const TrajOptDefaultSolverProfile&) = default; - TrajOptDefaultSolverProfile(TrajOptDefaultSolverProfile&&) = default; - TrajOptDefaultSolverProfile& operator=(TrajOptDefaultSolverProfile&&) = default; - /** @brief The Convex solver to use */ sco::ModelType convex_solver{ sco::ModelType::OSQP }; diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h index f9e332bd10..a5323148c9 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h @@ -52,10 +52,6 @@ class TrajOptPlanProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptPlanProfile(); - TrajOptPlanProfile(const TrajOptPlanProfile&) = delete; - TrajOptPlanProfile& operator=(const TrajOptPlanProfile&) = delete; - TrajOptPlanProfile(TrajOptPlanProfile&&) = delete; - TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -92,10 +88,6 @@ class TrajOptCompositeProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptCompositeProfile(); - TrajOptCompositeProfile(const TrajOptCompositeProfile&) = delete; - TrajOptCompositeProfile& operator=(const TrajOptCompositeProfile&) = delete; - TrajOptCompositeProfile(TrajOptCompositeProfile&&) = delete; - TrajOptCompositeProfile& operator=(TrajOptCompositeProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -125,10 +117,6 @@ class TrajOptSolverProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptSolverProfile(); - TrajOptSolverProfile(const TrajOptSolverProfile&) = delete; - TrajOptSolverProfile& operator=(const TrajOptSolverProfile&) = delete; - TrajOptSolverProfile(TrajOptSolverProfile&&) = delete; - TrajOptSolverProfile& operator=(TrajOptSolverProfile&&) = delete; /** * @brief A utility function for getting profile ID diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h index dd239dfc3c..c26929f793 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h @@ -30,6 +30,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tinyxml2 @@ -70,6 +72,11 @@ struct CollisionCostConfig double coeff = 20; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; /** @@ -97,7 +104,15 @@ struct CollisionConstraintConfig double coeff = 20; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::CollisionCostConfig) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::CollisionConstraintConfig) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_COLLISION_CONFIG_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h index eb57605eb9..e09acb3ae8 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h @@ -29,6 +29,8 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tinyxml2 @@ -70,6 +72,11 @@ struct CartesianWaypointConfig Eigen::Matrix coeff{ Eigen::VectorXd::Constant(6, 5) }; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; /** @@ -100,7 +107,15 @@ struct JointWaypointConfig Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::CartesianWaypointConfig) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::JointWaypointConfig) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp index 14f12308bd..8ee1838e8d 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -43,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include static const double LONGEST_VALID_SEGMENT_FRACTION_DEFAULT = 0.01; @@ -469,6 +471,21 @@ template void TrajOptDefaultCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptCompositeProfile); + ar& BOOST_SERIALIZATION_NVP(contact_test_type); + ar& BOOST_SERIALIZATION_NVP(collision_cost_config); + ar& BOOST_SERIALIZATION_NVP(collision_constraint_config); + ar& BOOST_SERIALIZATION_NVP(smooth_velocities); + ar& BOOST_SERIALIZATION_NVP(velocity_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_accelerations); + ar& BOOST_SERIALIZATION_NVP(acceleration_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_jerks); + ar& BOOST_SERIALIZATION_NVP(jerk_coeff); + ar& BOOST_SERIALIZATION_NVP(avoid_singularity); + ar& BOOST_SERIALIZATION_NVP(avoid_singularity_coeff); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_fraction); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(special_collision_cost); + ar& BOOST_SERIALIZATION_NVP(special_collision_constraint); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index 039cc388b8..63b96df67f 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -318,6 +318,11 @@ template void TrajOptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptPlanProfile); + ar& BOOST_SERIALIZATION_NVP(cartesian_cost_config); + ar& BOOST_SERIALIZATION_NVP(cartesian_constraint_config); + ar& BOOST_SERIALIZATION_NVP(joint_cost_config); + ar& BOOST_SERIALIZATION_NVP(joint_constraint_config); + // ar& BOOST_SERIALIZATION_NVP(constraint_error_functions); /** @todo FIX */ } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp index b620a18ce0..b38cbbdccd 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp @@ -50,6 +50,11 @@ template void TrajOptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptSolverProfile); + /** @todo FIX */ + // ar& BOOST_SERIALIZATION_NVP(convex_solver); + // ar& BOOST_SERIALIZATION_NVP(convex_solver_config); + // ar& BOOST_SERIALIZATION_NVP(opt_info); + // ar& BOOST_SERIALIZATION_NVP(callbacks); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp index c725c2b08c..8a12453479 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp @@ -29,6 +29,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -140,6 +141,17 @@ tinyxml2::XMLElement* CollisionCostConfig::toXML(tinyxml2::XMLDocument& doc) con return xml_coll_cost_config; } +template +void CollisionCostConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_weighted_sum); + ar& BOOST_SERIALIZATION_NVP(type); + ar& BOOST_SERIALIZATION_NVP(safety_margin); + ar& BOOST_SERIALIZATION_NVP(safety_margin_buffer); + ar& BOOST_SERIALIZATION_NVP(coeff); +} + CollisionConstraintConfig::CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); @@ -243,4 +255,21 @@ tinyxml2::XMLElement* CollisionConstraintConfig::toXML(tinyxml2::XMLDocument& do return xml_coll_cnt_config; } + +template +void CollisionConstraintConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_weighted_sum); + ar& BOOST_SERIALIZATION_NVP(type); + ar& BOOST_SERIALIZATION_NVP(safety_margin); + ar& BOOST_SERIALIZATION_NVP(safety_margin_buffer); + ar& BOOST_SERIALIZATION_NVP(coeff); +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CollisionCostConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CollisionCostConfig) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CollisionConstraintConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CollisionConstraintConfig) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp index e10e0ae846..f2db1b9fb6 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_planning { @@ -154,6 +155,16 @@ tinyxml2::XMLElement* CartesianWaypointConfig::toXML(tinyxml2::XMLDocument& doc) return xml_cartesian_waypoint_config; } +template +void CartesianWaypointConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_tolerance_override); + ar& BOOST_SERIALIZATION_NVP(lower_tolerance); + ar& BOOST_SERIALIZATION_NVP(upper_tolerance); + ar& BOOST_SERIALIZATION_NVP(coeff); +} + JointWaypointConfig::JointWaypointConfig(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); @@ -271,4 +282,20 @@ tinyxml2::XMLElement* JointWaypointConfig::toXML(tinyxml2::XMLDocument& doc) con return xml_cartesian_waypoint_config; } + +template +void JointWaypointConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_tolerance_override); + ar& BOOST_SERIALIZATION_NVP(lower_tolerance); + ar& BOOST_SERIALIZATION_NVP(upper_tolerance); + ar& BOOST_SERIALIZATION_NVP(coeff); +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CartesianWaypointConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CartesianWaypointConfig) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::JointWaypointConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::JointWaypointConfig) diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h index 4cb61a3a96..04ee00a461 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h @@ -53,8 +53,8 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile ~TrajOptIfoptDefaultSolverProfile() override; TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = delete; TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = delete; - TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = default; - TrajOptIfoptDefaultSolverProfile& operator=(TrajOptIfoptDefaultSolverProfile&&) = default; + TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = delete; + TrajOptIfoptDefaultSolverProfile&& operator=(TrajOptIfoptDefaultSolverProfile&&) = delete; /** @brief The OSQP convex solver settings to use * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h index 55fc37eb31..2aadefeb10 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h @@ -54,10 +54,6 @@ class TrajOptIfoptPlanProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptIfoptPlanProfile(); - TrajOptIfoptPlanProfile(const TrajOptIfoptPlanProfile&) = delete; - TrajOptIfoptPlanProfile& operator=(const TrajOptIfoptPlanProfile&) = delete; - TrajOptIfoptPlanProfile(TrajOptIfoptPlanProfile&&) = delete; - TrajOptIfoptPlanProfile& operator=(TrajOptIfoptPlanProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -94,10 +90,6 @@ class TrajOptIfoptCompositeProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptIfoptCompositeProfile(); - TrajOptIfoptCompositeProfile(const TrajOptIfoptCompositeProfile&) = delete; - TrajOptIfoptCompositeProfile& operator=(const TrajOptIfoptCompositeProfile&) = delete; - TrajOptIfoptCompositeProfile(TrajOptIfoptCompositeProfile&&) = delete; - TrajOptIfoptCompositeProfile& operator=(TrajOptIfoptCompositeProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -127,10 +119,6 @@ class TrajOptIfoptSolverProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptIfoptSolverProfile(); - TrajOptIfoptSolverProfile(const TrajOptIfoptSolverProfile&) = delete; - TrajOptIfoptSolverProfile& operator=(const TrajOptIfoptSolverProfile&) = delete; - TrajOptIfoptSolverProfile(TrajOptIfoptSolverProfile&&) = delete; - TrajOptIfoptSolverProfile& operator=(TrajOptIfoptSolverProfile&&) = delete; /** * @brief A utility function for getting profile ID diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp index fc837f100c..0a9ae17a8f 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp @@ -29,8 +29,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -38,6 +40,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include namespace tesseract_planning { @@ -103,6 +107,18 @@ template void TrajOptIfoptDefaultCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptCompositeProfile); + ar& BOOST_SERIALIZATION_NVP(collision_cost_config); + ar& BOOST_SERIALIZATION_NVP(collision_constraint_config); + ar& BOOST_SERIALIZATION_NVP(smooth_velocities); + ar& BOOST_SERIALIZATION_NVP(velocity_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_accelerations); + ar& BOOST_SERIALIZATION_NVP(acceleration_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_jerks); + ar& BOOST_SERIALIZATION_NVP(jerk_coeff); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_fraction); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(special_collision_cost); + ar& BOOST_SERIALIZATION_NVP(special_collision_constraint); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp index a2d6994a49..4627f90d3c 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp @@ -43,6 +43,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + namespace tesseract_planning { void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, @@ -138,6 +140,9 @@ template void TrajOptIfoptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptPlanProfile); + ar& BOOST_SERIALIZATION_NVP(cartesian_coeff); + ar& BOOST_SERIALIZATION_NVP(joint_coeff); + ar& BOOST_SERIALIZATION_NVP(term_type); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp index fff3fd303e..e6471c6ddd 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -68,6 +69,10 @@ template void TrajOptIfoptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptSolverProfile); + /** @todo FIX */ + // ar& BOOST_SERIALIZATION_NVP(convex_solver_settings); + // ar& BOOST_SERIALIZATION_NVP(opt_info); + // ar& BOOST_SERIALIZATION_NVP(callbacks); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index 6fd736e5fb..d42dc32dd0 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -38,8 +38,7 @@ set(LIB_SOURCE_FILES src/profiles/profile_switch_profile.cpp src/profiles/ruckig_trajectory_smoothing_profile.cpp src/profiles/time_optimal_parameterization_profile.cpp - src/profiles/upsample_trajectory_profile.cpp - ) + src/profiles/upsample_trajectory_profile.cpp) set(LIB_SOURCE_LINK_LIBRARIES ${PROJECT_NAME} diff --git a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp index 1358f2f501..5288c3e0d3 100644 --- a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { @@ -60,6 +61,7 @@ template void ContactCheckProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(config); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp b/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp index 82dc182168..20d830f90a 100644 --- a/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp @@ -40,6 +40,10 @@ template void FixStateBoundsProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(mode); + ar& BOOST_SERIALIZATION_NVP(max_deviation_global); + ar& BOOST_SERIALIZATION_NVP(upper_bounds_reduction); + ar& BOOST_SERIALIZATION_NVP(lower_bounds_reduction); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp b/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp index 6c5f5e3a9b..3ee61e3634 100644 --- a/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp @@ -25,8 +25,10 @@ */ #include +#include #include #include +#include #include namespace tesseract_planning @@ -47,6 +49,11 @@ template void FixStateCollisionProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(mode); + ar& BOOST_SERIALIZATION_NVP(correction_workflow); + ar& BOOST_SERIALIZATION_NVP(jiggle_factor); + ar& BOOST_SERIALIZATION_NVP(collision_check_config); + ar& BOOST_SERIALIZATION_NVP(sampling_attempts); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp b/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp index edcf3f7d05..15dfe2a2db 100644 --- a/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp @@ -53,6 +53,8 @@ template void IterativeSplineParameterizationProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp index eeff731d36..8036add145 100644 --- a/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp @@ -44,6 +44,7 @@ template void MinLengthProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(min_length); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp index b70db9798a..f6ce5d1e07 100644 --- a/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp @@ -40,6 +40,7 @@ template void ProfileSwitchProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(return_value); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp index d677491147..12113f0cf6 100644 --- a/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp @@ -73,6 +73,9 @@ template void RuckigTrajectorySmoothingMoveProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_jerk_scaling_factor); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp b/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp index 04974fc0a2..63b2c838f3 100644 --- a/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp @@ -60,6 +60,11 @@ template void TimeOptimalParameterizationProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_jerk_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(path_tolerance); + ar& BOOST_SERIALIZATION_NVP(min_angle_change); } } // namespace tesseract_planning diff --git a/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp index 10e536b0ba..514400a763 100644 --- a/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp @@ -46,6 +46,7 @@ template void UpsampleTrajectoryProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_length); } } // namespace tesseract_planning