diff --git a/tesseract_command_language/include/tesseract_command_language/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h index 2a97d726d2..c4ed36a8b4 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile.h +++ b/tesseract_command_language/include/tesseract_command_language/profile.h @@ -32,9 +32,7 @@ namespace tesseract_planning { -/** - * @brief The Profile class - */ +/** @brief The Profile class */ class Profile { public: @@ -42,21 +40,13 @@ class Profile using ConstPtr = std::shared_ptr; Profile() = default; - Profile(std::size_t key); Profile(const Profile&) = delete; Profile& operator=(const Profile&) = delete; Profile(Profile&&) = delete; Profile&& operator=(Profile&&) = delete; virtual ~Profile() = default; - /** - * @brief Get the hash code associated with the profile - * @return The profile's hash code - */ - std::size_t getKey() const; - protected: - std::size_t key_{ 0 }; friend class boost::serialization::access; template void serialize(Archive&, const unsigned int); // NOLINT 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 f3e58c4ba5..849dddbd88 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -69,61 +69,54 @@ class ProfileDictionary /** * @brief Check if a profile exists * @details If profile entry does not exist it also returns false - * @param key The profile key * @param ns The profile namespace * @param profile_name The profile name * @return True if profile exists, otherwise false */ - bool hasProfile(std::size_t key, const std::string& ns, const std::string& profile_name) const; + bool hasProfile(const std::string& ns, const std::string& profile_name) const; /** * @brief Get a profile by name * @details Check if the profile exist before calling this function, if missing an exception is thrown - * @param key The profile key * @param ns The profile namespace * @param profile_name The profile name * @return The profile */ - Profile::ConstPtr getProfile(std::size_t key, const std::string& ns, const std::string& profile_name) const; + Profile::ConstPtr getProfile(const std::string& ns, const std::string& profile_name) const; /** * @brief Remove a profile - * @param key The profile key * @param ns The profile namespace * @param profile_name The profile to be removed */ - void removeProfile(std::size_t key, const std::string& ns, const std::string& profile_name); + void removeProfile(const std::string& ns, const std::string& profile_name); /** * @brief Check if a profile entry exists - * @param key The profile key * @param ns The profile namespace * @return True if exists, otherwise false */ - bool hasProfileEntry(std::size_t key, const std::string& ns) const; + bool hasProfileEntry(const std::string& ns) const; /** * @brief Remove a profile entry - * @param key The profile key * @param ns The profile namespace */ - void removeProfileEntry(std::size_t key, const std::string& ns); + void removeProfileEntry(const std::string& ns); /** * @brief Get a profile entry - * @param key The profile key * @param ns The profile namespace * @return The profile map associated with the profile entry */ - std::unordered_map getProfileEntry(std::size_t key, const std::string& ns) const; + std::unordered_map getProfileEntry(const std::string& ns) const; /** @brief Clear the dictionary */ void clear(); protected: mutable std::shared_mutex mutex_; - std::unordered_map>> - profiles_; + std::unordered_map> profiles_; friend class boost::serialization::access; template 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 73e0be72e7..e599918da6 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,168 +63,169 @@ 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()); -} - -{ // 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()); -} + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T(pose) }; + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_FALSE(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()); - - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_TRUE(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()); - 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 -{ // 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 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); + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); } - { // 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 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 clear seed - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.getSeed() = seed_state; - EXPECT_TRUE(wp.hasSeed()); - wp.clearSeed(); - EXPECT_FALSE(wp.hasSeed()); + { // 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()); + } } } -} } // 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 5353ab1545..ca119e4d08 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,227 +52,228 @@ 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 - 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 + { // Test is constrained JointWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); + EXPECT_FALSE(wp.isConstrained()); + wp.setIsConstrained(true); + EXPECT_TRUE(wp.isConstrained()); } - { // Test assigning - JointWaypointPoly 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 + 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); + } } -} -{ // Set/Get Positions - Eigen::VectorXd positions; - positions.resize(3); - positions << 1.0, 2.0, 3.0; + { // 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)); - } + { // Test set + JointWaypointPoly wp{ T() }; + wp.setPosition(positions); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + } - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getPosition() = positions; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + { // Test assigning + JointWaypointPoly wp{ T() }; + wp.getPosition() = positions; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + } } -} - -{ // 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_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 -{ // 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); + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); } - { - 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); + { // 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); + } } } -} } // 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 08a14b851a..991a77d5c5 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,256 +52,257 @@ 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 + { // 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); - } + { // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set + StateWaypointPoly wp{ T() }; + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); + } } -} -{ // Set/Get Positions - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Positions + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setPosition(data); - EXPECT_TRUE(wp.getPosition().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setPosition(data); + EXPECT_TRUE(wp.getPosition().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getPosition() = data; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getPosition() = data; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + } } -} -{ // Set/Get Velocity - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Velocity + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setVelocity(data); - EXPECT_TRUE(wp.getVelocity().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setVelocity(data); + EXPECT_TRUE(wp.getVelocity().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getVelocity() = data; - EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getVelocity() = data; + EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + } } -} -{ // Set/Get Acceleration - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Acceleration + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setAcceleration(data); - EXPECT_TRUE(wp.getAcceleration().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setAcceleration(data); + EXPECT_TRUE(wp.getAcceleration().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getAcceleration() = data; - EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getAcceleration() = data; + EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + } } -} -{ // Set/Get Effort - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Effort + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setEffort(data); - EXPECT_TRUE(wp.getEffort().isApprox(data)); + { // Test set + StateWaypointPoly wp{ T() }; + wp.setEffort(data); + EXPECT_TRUE(wp.getEffort().isApprox(data)); + } + + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getEffort() = data; + EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); + } } - { // Test assigning + { // Set/Get Time + double data{ 3 }; StateWaypointPoly wp{ T() }; - wp.getEffort() = data; - EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); + wp.setTime(data); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 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.cpp b/tesseract_command_language/src/profile.cpp index 52f1eee0b6..be8415079a 100644 --- a/tesseract_command_language/src/profile.cpp +++ b/tesseract_command_language/src/profile.cpp @@ -31,14 +31,9 @@ namespace tesseract_planning { -Profile::Profile(std::size_t key) : key_(key) {} - -std::size_t Profile::getKey() const { return key_; } - template -void Profile::serialize(Archive& ar, const unsigned int /*version*/) +void Profile::serialize(Archive& /*ar*/, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp("key", key_); } } // namespace tesseract_planning diff --git a/tesseract_command_language/src/profile_dictionary.cpp b/tesseract_command_language/src/profile_dictionary.cpp index 30f3ea26b5..b5c3ede5eb 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -31,17 +31,17 @@ namespace tesseract_planning { -bool ProfileDictionary::hasProfileEntry(std::size_t key, const std::string& ns) const +bool ProfileDictionary::hasProfileEntry(const std::string& ns) const { const std::shared_lock lock(mutex_); auto it = profiles_.find(ns); if (it == profiles_.end()) return false; - return (it->second.find(key) != it->second.end()); + return true; } -void ProfileDictionary::removeProfileEntry(std::size_t key, const std::string& ns) +void ProfileDictionary::removeProfileEntry(const std::string& ns) { const std::unique_lock lock(mutex_); @@ -49,23 +49,17 @@ void ProfileDictionary::removeProfileEntry(std::size_t key, const std::string& n if (it == profiles_.end()) return; - it->second.erase(key); + profiles_.erase(ns); } -std::unordered_map ProfileDictionary::getProfileEntry(std::size_t key, - const std::string& ns) const +std::unordered_map ProfileDictionary::getProfileEntry(const std::string& ns) const { const std::shared_lock lock(mutex_); auto it = profiles_.find(ns); if (it == profiles_.end()) throw std::runtime_error("Profile namespace does not exist for '" + ns + "'!"); - auto it2 = it->second.find(key); - if (it2 != it->second.end()) - return it2->second; - - throw std::runtime_error("Profile entry does not exist for type name '" + std::to_string(key) + "' in namespace '" + - ns + "'!"); + return it->second; } void ProfileDictionary::addProfile(const std::string& ns, @@ -87,59 +81,42 @@ void ProfileDictionary::addProfile(const std::string& ns, { std::unordered_map new_entry; new_entry[profile_name] = profile; - profiles_[ns][profile->getKey()] = new_entry; + profiles_[ns] = new_entry; } else { - auto it2 = it->second.find(profile->getKey()); - if (it2 != it->second.end()) - { - it2->second[profile_name] = profile; - } - else - { - std::unordered_map new_entry; - new_entry[profile_name] = profile; - it->second[profile->getKey()] = new_entry; - } + it->second[profile_name] = profile; } } -bool ProfileDictionary::hasProfile(std::size_t key, const std::string& ns, const std::string& profile_name) const +bool ProfileDictionary::hasProfile(const std::string& ns, const std::string& profile_name) const { const std::shared_lock lock(mutex_); auto it = profiles_.find(ns); if (it == profiles_.end()) return false; - auto it2 = it->second.find(key); + auto it2 = it->second.find(profile_name); if (it2 != it->second.end()) - { - auto it3 = it2->second.find(profile_name); - if (it3 != it2->second.end()) - return true; - } + return true; + return false; } -Profile::ConstPtr ProfileDictionary::getProfile(std::size_t key, - const std::string& ns, - const std::string& profile_name) const +Profile::ConstPtr ProfileDictionary::getProfile(const std::string& ns, const std::string& profile_name) const { const std::shared_lock lock(mutex_); - return profiles_.at(ns).at(key).at(profile_name); + return profiles_.at(ns).at(profile_name); } -void ProfileDictionary::removeProfile(std::size_t key, const std::string& ns, const std::string& profile_name) +void ProfileDictionary::removeProfile(const std::string& ns, const std::string& profile_name) { const std::unique_lock lock(mutex_); auto it = profiles_.find(ns); if (it == profiles_.end()) return; - auto it2 = it->second.find(key); - if (it2 != it->second.end()) - it2->second.erase(profile_name); + it->second.erase(profile_name); } void ProfileDictionary::clear() diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h index 9d0eb86da1..1058a09b94 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h @@ -138,20 +138,18 @@ std::shared_ptr getProfile(const std::string& ns, const ProfileDictionary& profile_dictionary, std::shared_ptr default_profile = nullptr) { - if (profile_dictionary.hasProfile(ProfileType::getStaticKey(), ns, profile)) - return std::static_pointer_cast( - profile_dictionary.getProfile(ProfileType::getStaticKey(), ns, profile)); + if (profile_dictionary.hasProfile(ns, profile)) + return std::static_pointer_cast(profile_dictionary.getProfile(ns, profile)); - CONSOLE_BRIDGE_logDebug("Profile '%s' was not found in namespace '%s' for type '%s'. Using default if available. " + CONSOLE_BRIDGE_logDebug("Profile '%s' was not found in namespace '%s'. Using default if available. " "Available " "profiles:", profile.c_str(), - ns.c_str(), - typeid(ProfileType).name()); + ns.c_str()); - if (profile_dictionary.hasProfileEntry(ProfileType::getStaticKey(), ns)) + if (profile_dictionary.hasProfileEntry(ns)) { - for (const auto& pair : profile_dictionary.getProfileEntry(ProfileType::getStaticKey(), ns)) + for (const auto& pair : profile_dictionary.getProfileEntry(ns)) { CONSOLE_BRIDGE_logDebug("%s", pair.first.c_str()); } @@ -178,8 +176,8 @@ std::shared_ptr applyProfileOverrides(const std::string& ns, if (!overrides) return nominal_profile; - if (overrides->hasProfile(ProfileType::getStaticKey(), ns, profile)) - return std::static_pointer_cast(overrides->getProfile(ProfileType::getStaticKey(), ns, profile)); + if (overrides->hasProfile(ns, profile)) + return std::static_pointer_cast(overrides->getProfile(ns, profile)); return nominal_profile; } diff --git a/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp index ea0c4cbe6b..8a7ee0b4c6 100644 --- a/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp @@ -33,14 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP struct ProfileBase : public tesseract_planning::Profile { - ProfileBase() : Profile(ProfileBase::getStaticKey()) {} - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey() { return std::type_index(typeid(ProfileBase)).hash_code(); } - int a{ 0 }; }; @@ -52,14 +44,6 @@ struct ProfileTest : public ProfileBase struct ProfileBase2 : public tesseract_planning::Profile { - ProfileBase2() : Profile(ProfileBase2::getStaticKey()) {} - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey() { return std::type_index(typeid(ProfileBase2)).hash_code(); } - int b{ 0 }; }; @@ -75,12 +59,12 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT { ProfileDictionary profiles; - EXPECT_FALSE(profiles.hasProfileEntry(ProfileBase::getStaticKey(), "ns")); + EXPECT_FALSE(profiles.hasProfileEntry("ns")); profiles.addProfile("ns", "key", std::make_shared()); - EXPECT_TRUE(profiles.hasProfileEntry(ProfileBase::getStaticKey(), "ns")); - EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "ns", "key")); + EXPECT_TRUE(profiles.hasProfileEntry("ns")); + EXPECT_TRUE(profiles.hasProfile("ns", "key")); auto profile = getProfile("ns", "key", profiles); @@ -89,19 +73,19 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT // Check add same profile with different key profiles.addProfile("ns", "key2", profile); - EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "ns", "key2")); + EXPECT_TRUE(profiles.hasProfile("ns", "key2")); auto profile2 = getProfile("ns", "key2", profiles); EXPECT_TRUE(profile2 != nullptr); EXPECT_EQ(profile2->a, 0); // Check replacing a profile profiles.addProfile("ns", "key", std::make_shared(10)); - EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "ns", "key")); + EXPECT_TRUE(profiles.hasProfile("ns", "key")); auto profile_check = getProfile("ns", "key", profiles); EXPECT_TRUE(profile_check != nullptr); EXPECT_EQ(profile_check->a, 10); - auto profile_map = profiles.getProfileEntry(ProfileBase::getStaticKey(), "ns"); + auto profile_map = profiles.getProfileEntry("ns"); auto it = profile_map.find("key"); EXPECT_TRUE(it != profile_map.end()); EXPECT_EQ(std::static_pointer_cast(it->second)->a, 10); @@ -112,13 +96,13 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT EXPECT_EQ(profile_check2->a, 20); // Request a profile entry namespace that does not exist - EXPECT_ANY_THROW(profiles.getProfileEntry(0, "DoesNotExist")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfileEntry("DoesNotExist")); // NOLINT // Request a profile that does not exist - EXPECT_ANY_THROW(profiles.getProfile(ProfileBase::getStaticKey(), "DoesNotExist", "key")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile("DoesNotExist", "key")); // NOLINT // Request a profile that does not exist - EXPECT_ANY_THROW(profiles.getProfile(ProfileBase::getStaticKey(), "ns", "DoesNotExist")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile("ns", "DoesNotExist")); // NOLINT // Check adding a empty namespace EXPECT_ANY_THROW(profiles.addProfile("", "key3", std::make_shared(20))); // NOLINT @@ -131,8 +115,8 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT // Add different profile entry profiles.addProfile("ns", "key", std::make_shared(5)); - EXPECT_TRUE(profiles.hasProfileEntry(ProfileBase2::getStaticKey(), "ns")); - EXPECT_TRUE(profiles.hasProfile(ProfileBase2::getStaticKey(), "ns", "key")); + EXPECT_TRUE(profiles.hasProfileEntry("ns")); + EXPECT_TRUE(profiles.hasProfile("ns", "key")); auto profile_check3 = getProfile("ns", "key", profiles); EXPECT_TRUE(profile_check3 != nullptr); EXPECT_EQ(profile_check3->b, 5); 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 8f90e755ff..df55cb7519 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 @@ -29,7 +29,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -53,13 +52,7 @@ class DescartesPlanProfile : public Profile using Ptr = std::shared_ptr>; using ConstPtr = std::shared_ptr>; - DescartesPlanProfile() : Profile(DescartesPlanProfile::getStaticKey()) {} - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey() { return std::type_index(typeid(DescartesPlanProfile)).hash_code(); } + DescartesPlanProfile() = default; virtual void apply(DescartesProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, 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 1d5822238a..7d1f4e4e6b 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 @@ -52,13 +52,7 @@ class OMPLPlanProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - OMPLPlanProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + OMPLPlanProfile() = default; virtual void setup(OMPLProblem& prob) const = 0; diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp index 2763116d0b..06ece935ca 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp @@ -27,14 +27,9 @@ #include #include #include -#include namespace tesseract_planning { -OMPLPlanProfile::OMPLPlanProfile() : Profile(OMPLPlanProfile::getStaticKey()) {} - -std::size_t OMPLPlanProfile::getStaticKey() { return std::type_index(typeid(OMPLPlanProfile)).hash_code(); } - template void OMPLPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { 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 05dd83a33e..59fd0ca7a0 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 @@ -49,13 +49,7 @@ class SimplePlannerPlanProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SimplePlannerPlanProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + SimplePlannerPlanProfile() = default; /** * @brief Generate a seed for the provided base_instruction @@ -88,13 +82,7 @@ class SimplePlannerCompositeProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SimplePlannerCompositeProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + SimplePlannerCompositeProfile() = default; // This contains functions for composite processing. Get start for example protected: diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp index fa055489a0..8880b7b2b4 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp @@ -26,32 +26,15 @@ #include #include #include -#include namespace tesseract_planning { -SimplePlannerPlanProfile::SimplePlannerPlanProfile() : Profile(SimplePlannerPlanProfile::getStaticKey()) {} - -std::size_t SimplePlannerPlanProfile::getStaticKey() -{ - return std::type_index(typeid(SimplePlannerPlanProfile)).hash_code(); -} - template void SimplePlannerPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); } -SimplePlannerCompositeProfile::SimplePlannerCompositeProfile() : Profile(SimplePlannerCompositeProfile::getStaticKey()) -{ -} - -std::size_t SimplePlannerCompositeProfile::getStaticKey() -{ - return std::type_index(typeid(SimplePlannerCompositeProfile)).hash_code(); -} - template void SimplePlannerCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) { 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 a5323148c9..7b67181c3a 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 @@ -51,13 +51,7 @@ class TrajOptPlanProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptPlanProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + TrajOptPlanProfile() = default; virtual void apply(trajopt::ProblemConstructionInfo& pci, const CartesianWaypointPoly& cartesian_waypoint, @@ -87,13 +81,7 @@ class TrajOptCompositeProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptCompositeProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + TrajOptCompositeProfile() = default; virtual void apply(trajopt::ProblemConstructionInfo& pci, int start_index, @@ -116,13 +104,7 @@ class TrajOptSolverProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptSolverProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + TrajOptSolverProfile() = default; virtual void apply(trajopt::ProblemConstructionInfo& pci) const = 0; diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp index 190c23d1e4..11948db1eb 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp @@ -26,37 +26,21 @@ #include #include #include -#include namespace tesseract_planning { -TrajOptPlanProfile::TrajOptPlanProfile() : Profile(TrajOptPlanProfile::getStaticKey()) {} - -std::size_t TrajOptPlanProfile::getStaticKey() { return std::type_index(typeid(TrajOptPlanProfile)).hash_code(); } - template void TrajOptPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); } -TrajOptCompositeProfile::TrajOptCompositeProfile() : Profile(TrajOptCompositeProfile::getStaticKey()) {} - -std::size_t TrajOptCompositeProfile::getStaticKey() -{ - return std::type_index(typeid(TrajOptCompositeProfile)).hash_code(); -} - template void TrajOptCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); } -TrajOptSolverProfile::TrajOptSolverProfile() : Profile(TrajOptSolverProfile::getStaticKey()) {} - -std::size_t TrajOptSolverProfile::getStaticKey() { return std::type_index(typeid(TrajOptSolverProfile)).hash_code(); } - template void TrajOptSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) { 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 2aadefeb10..409367af8d 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 @@ -53,13 +53,7 @@ class TrajOptIfoptPlanProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptPlanProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + TrajOptIfoptPlanProfile() = default; virtual void apply(TrajOptIfoptProblem& problem, const CartesianWaypointPoly& cartesian_waypoint, @@ -89,13 +83,7 @@ class TrajOptIfoptCompositeProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptCompositeProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + TrajOptIfoptCompositeProfile() = default; virtual void apply(TrajOptIfoptProblem& problem, int start_index, @@ -118,13 +106,7 @@ class TrajOptIfoptSolverProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptSolverProfile(); - - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); + TrajOptIfoptSolverProfile() = default; virtual void apply(TrajOptIfoptProblem& problem) const = 0; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp index 5f5f7dac86..f5f2084fda 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp @@ -26,43 +26,21 @@ #include #include #include -#include namespace tesseract_planning { -TrajOptIfoptPlanProfile::TrajOptIfoptPlanProfile() : Profile(TrajOptIfoptPlanProfile::getStaticKey()) {} - -std::size_t TrajOptIfoptPlanProfile::getStaticKey() -{ - return std::type_index(typeid(TrajOptIfoptPlanProfile)).hash_code(); -} - template void TrajOptIfoptPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); } -TrajOptIfoptCompositeProfile::TrajOptIfoptCompositeProfile() : Profile(TrajOptIfoptCompositeProfile::getStaticKey()) {} - -std::size_t TrajOptIfoptCompositeProfile::getStaticKey() -{ - return std::type_index(typeid(TrajOptIfoptCompositeProfile)).hash_code(); -} - template void TrajOptIfoptCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); } -TrajOptIfoptSolverProfile::TrajOptIfoptSolverProfile() : Profile(TrajOptIfoptSolverProfile::getStaticKey()) {} - -std::size_t TrajOptIfoptSolverProfile::getStaticKey() -{ - return std::type_index(typeid(TrajOptIfoptSolverProfile)).hash_code(); -} - template void TrajOptIfoptSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h index c81f96e55b..f7cecf838b 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h @@ -44,12 +44,6 @@ struct ContactCheckProfile : public Profile ContactCheckProfile(); ContactCheckProfile(double longest_valid_segment_length, double contact_distance); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - /** @brief The contact manager config */ tesseract_collision::CollisionCheckConfig config; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h index 2f85a566f8..df001aeff4 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h @@ -51,12 +51,6 @@ struct FixStateBoundsProfile : public Profile FixStateBoundsProfile(Settings mode = Settings::ALL); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - /** @brief Sets which terms will be corrected */ Settings mode; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h index 7f37d1536f..ab62385efb 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h @@ -63,12 +63,6 @@ struct FixStateCollisionProfile : public Profile FixStateCollisionProfile(Settings mode = Settings::ALL); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - /** @brief Sets which terms will be corrected */ Settings mode; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h index dcb7bc4301..a1c07dac2c 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h @@ -40,15 +40,9 @@ struct IterativeSplineParameterizationProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - IterativeSplineParameterizationProfile(); + IterativeSplineParameterizationProfile() = default; IterativeSplineParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - /** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */ double max_velocity_scaling_factor{ 1.0 }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h index adfd5d9ec8..6fbcfb622a 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h @@ -42,15 +42,9 @@ struct MinLengthProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - MinLengthProfile(); + MinLengthProfile() = default; MinLengthProfile(long min_length); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - long min_length{ 10 }; protected: diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h index e1b2addc78..681817c91e 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h @@ -40,12 +40,6 @@ struct ProfileSwitchProfile : public Profile ProfileSwitchProfile(int return_value = 1); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - int return_value; protected: diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h index 8b99bce65b..7339e8d34a 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h @@ -38,18 +38,12 @@ struct RuckigTrajectorySmoothingCompositeProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - RuckigTrajectorySmoothingCompositeProfile(); + RuckigTrajectorySmoothingCompositeProfile() = default; RuckigTrajectorySmoothingCompositeProfile(double duration_extension_fraction, double max_duration_extension_factor, double max_velocity_scaling_factor, double max_acceleration_scaling_factor); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - /** @brief duration_extension_fraction The amount to scale the trajectory each time */ double duration_extension_fraction{ 1.1 }; @@ -64,6 +58,11 @@ struct RuckigTrajectorySmoothingCompositeProfile : public Profile /** @brief max_jerk_scaling_factor The max jerk scaling factor passed to the solver */ double max_jerk_scaling_factor{ 1.0 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct RuckigTrajectorySmoothingMoveProfile : public Profile @@ -71,15 +70,9 @@ struct RuckigTrajectorySmoothingMoveProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - RuckigTrajectorySmoothingMoveProfile(); + RuckigTrajectorySmoothingMoveProfile() = default; RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - /** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */ double max_velocity_scaling_factor{ 1.0 }; @@ -96,6 +89,7 @@ struct RuckigTrajectorySmoothingMoveProfile : public Profile }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RuckigTrajectorySmoothingCompositeProfile) BOOST_CLASS_EXPORT_KEY(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) #endif // TESSERACT_TASK_COMPOSER_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h index afc29a3677..16de49e266 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h @@ -41,19 +41,13 @@ struct TimeOptimalParameterizationProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TimeOptimalParameterizationProfile(); + TimeOptimalParameterizationProfile() = default; TimeOptimalParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, double max_jerk_scaling_factor, double path_tolerance, double min_angle_change); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - /** @brief The max velocity scaling factor passed to the solver. Default: 1.0*/ double max_velocity_scaling_factor{ 1.0 }; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h index 6bd56752ae..43e7d523c4 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h @@ -39,15 +39,9 @@ struct UpsampleTrajectoryProfile : public Profile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - UpsampleTrajectoryProfile(); + UpsampleTrajectoryProfile() = default; UpsampleTrajectoryProfile(double longest_valid_segment_length); - /** - * @brief A utility function for getting profile ID - * @return The profile ID used when storing in profile dictionary - */ - static std::size_t getStaticKey(); - double longest_valid_segment_length{ 0.1 }; protected: 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 5288c3e0d3..3d6fc80a5c 100644 --- a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp @@ -27,7 +27,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -40,7 +39,6 @@ namespace tesseract_planning ContactCheckProfile::ContactCheckProfile() : ContactCheckProfile(0.05, 0) {} ContactCheckProfile::ContactCheckProfile(double longest_valid_segment_length, double contact_distance) - : Profile(ContactCheckProfile::getStaticKey()) { config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; config.longest_valid_segment_length = longest_valid_segment_length; @@ -55,8 +53,6 @@ ContactCheckProfile::ContactCheckProfile(double longest_valid_segment_length, do } } -std::size_t ContactCheckProfile::getStaticKey() { return std::type_index(typeid(ContactCheckProfile)).hash_code(); } - template void ContactCheckProfile::serialize(Archive& ar, const unsigned int /*version*/) { 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 20d830f90a..745b5abfb5 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 @@ -26,15 +26,10 @@ #include #include #include -#include namespace tesseract_planning { -FixStateBoundsProfile::FixStateBoundsProfile(Settings mode) : Profile(FixStateBoundsProfile::getStaticKey()), mode(mode) -{ -} - -std::size_t FixStateBoundsProfile::getStaticKey() { return std::type_index(typeid(FixStateBoundsProfile)).hash_code(); } +FixStateBoundsProfile::FixStateBoundsProfile(Settings mode) : mode(mode) {} template void FixStateBoundsProfile::serialize(Archive& ar, const unsigned int /*version*/) 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 3ee61e3634..f5d7fd98e9 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 @@ -29,22 +29,15 @@ #include #include #include -#include namespace tesseract_planning { -FixStateCollisionProfile::FixStateCollisionProfile(Settings mode) - : Profile(FixStateCollisionProfile::getStaticKey()), mode(mode) +FixStateCollisionProfile::FixStateCollisionProfile(Settings mode) : mode(mode) { collision_check_config.contact_request.type = tesseract_collision::ContactTestType::FIRST; collision_check_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; } -std::size_t FixStateCollisionProfile::getStaticKey() -{ - return std::type_index(typeid(FixStateCollisionProfile)).hash_code(); -} - template void FixStateCollisionProfile::serialize(Archive& ar, const unsigned int /*version*/) { 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 15dfe2a2db..4b47b3c13a 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 @@ -27,28 +27,16 @@ #include #include #include -#include namespace tesseract_planning { -IterativeSplineParameterizationProfile::IterativeSplineParameterizationProfile() - : Profile(IterativeSplineParameterizationProfile::getStaticKey()) -{ -} - IterativeSplineParameterizationProfile::IterativeSplineParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor) - : Profile(IterativeSplineParameterizationProfile::getStaticKey()) - , max_velocity_scaling_factor(max_velocity_scaling_factor) + : max_velocity_scaling_factor(max_velocity_scaling_factor) , max_acceleration_scaling_factor(max_acceleration_scaling_factor) { } -std::size_t IterativeSplineParameterizationProfile::getStaticKey() -{ - return std::type_index(typeid(IterativeSplineParameterizationProfile)).hash_code(); -} - template void IterativeSplineParameterizationProfile::serialize(Archive& ar, const unsigned int /*version*/) { 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 8036add145..6ed3b7f897 100644 --- a/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp @@ -29,16 +29,10 @@ #include #include #include -#include namespace tesseract_planning { -MinLengthProfile::MinLengthProfile() : Profile(MinLengthProfile::getStaticKey()) {} -MinLengthProfile::MinLengthProfile(long min_length) : Profile(MinLengthProfile::getStaticKey()), min_length(min_length) -{ -} - -std::size_t MinLengthProfile::getStaticKey() { return std::type_index(typeid(MinLengthProfile)).hash_code(); } +MinLengthProfile::MinLengthProfile(long min_length) : min_length(min_length) {} template void MinLengthProfile::serialize(Archive& ar, const unsigned int /*version*/) 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 f6ce5d1e07..aa65ec4042 100644 --- a/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp @@ -25,16 +25,10 @@ #include #include #include -#include namespace tesseract_planning { -ProfileSwitchProfile::ProfileSwitchProfile(int return_value) - : Profile(ProfileSwitchProfile::getStaticKey()), return_value(return_value) -{ -} - -std::size_t ProfileSwitchProfile::getStaticKey() { return std::type_index(typeid(ProfileSwitchProfile)).hash_code(); } +ProfileSwitchProfile::ProfileSwitchProfile(int return_value) : return_value(return_value) {} template void ProfileSwitchProfile::serialize(Archive& ar, const unsigned int /*version*/) 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 12113f0cf6..1cfd6cc245 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 @@ -25,50 +25,39 @@ #include #include #include -#include namespace tesseract_planning { -RuckigTrajectorySmoothingCompositeProfile::RuckigTrajectorySmoothingCompositeProfile() - : Profile(RuckigTrajectorySmoothingCompositeProfile::getStaticKey()) -{ -} - RuckigTrajectorySmoothingCompositeProfile::RuckigTrajectorySmoothingCompositeProfile( double duration_extension_fraction, double max_duration_extension_factor, double max_velocity_scaling_factor, double max_acceleration_scaling_factor) - : Profile(RuckigTrajectorySmoothingCompositeProfile::getStaticKey()) - , duration_extension_fraction(duration_extension_fraction) + : duration_extension_fraction(duration_extension_fraction) , max_duration_extension_factor(max_duration_extension_factor) , max_velocity_scaling_factor(max_velocity_scaling_factor) , max_acceleration_scaling_factor(max_acceleration_scaling_factor) { } -std::size_t RuckigTrajectorySmoothingCompositeProfile::getStaticKey() +template +void RuckigTrajectorySmoothingCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) { - return std::type_index(typeid(RuckigTrajectorySmoothingCompositeProfile)).hash_code(); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(duration_extension_fraction); + ar& BOOST_SERIALIZATION_NVP(max_duration_extension_factor); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_jerk_scaling_factor); } -RuckigTrajectorySmoothingMoveProfile::RuckigTrajectorySmoothingMoveProfile() - : Profile(RuckigTrajectorySmoothingMoveProfile::getStaticKey()) -{ -} RuckigTrajectorySmoothingMoveProfile::RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor) - : Profile(RuckigTrajectorySmoothingMoveProfile::getStaticKey()) - , max_velocity_scaling_factor(max_velocity_scaling_factor) + : max_velocity_scaling_factor(max_velocity_scaling_factor) , max_acceleration_scaling_factor(max_acceleration_scaling_factor) { } -std::size_t RuckigTrajectorySmoothingMoveProfile::getStaticKey() -{ - return std::type_index(typeid(RuckigTrajectorySmoothingMoveProfile)).hash_code(); -} - template void RuckigTrajectorySmoothingMoveProfile::serialize(Archive& ar, const unsigned int /*version*/) { @@ -81,5 +70,7 @@ void RuckigTrajectorySmoothingMoveProfile::serialize(Archive& ar, const unsigned } // namespace tesseract_planning #include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RuckigTrajectorySmoothingCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingCompositeProfile) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) 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 63b2c838f3..e957e91ceb 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 @@ -28,22 +28,15 @@ #include #include #include -#include namespace tesseract_planning { -TimeOptimalParameterizationProfile::TimeOptimalParameterizationProfile() - : Profile(TimeOptimalParameterizationProfile::getStaticKey()) -{ -} - TimeOptimalParameterizationProfile::TimeOptimalParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, double max_jerk_scaling_factor, double path_tolerance, double min_angle_change) - : Profile(TimeOptimalParameterizationProfile::getStaticKey()) - , max_velocity_scaling_factor(max_velocity_scaling_factor) + : max_velocity_scaling_factor(max_velocity_scaling_factor) , max_acceleration_scaling_factor(max_acceleration_scaling_factor) , max_jerk_scaling_factor(max_jerk_scaling_factor) , path_tolerance(path_tolerance) @@ -51,11 +44,6 @@ TimeOptimalParameterizationProfile::TimeOptimalParameterizationProfile(double ma { } -std::size_t TimeOptimalParameterizationProfile::getStaticKey() -{ - return std::type_index(typeid(TimeOptimalParameterizationProfile)).hash_code(); -} - template void TimeOptimalParameterizationProfile::serialize(Archive& ar, const unsigned int /*version*/) { 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 514400a763..261c68d749 100644 --- a/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp @@ -26,20 +26,12 @@ #include #include #include -#include namespace tesseract_planning { -UpsampleTrajectoryProfile::UpsampleTrajectoryProfile() : Profile(UpsampleTrajectoryProfile::getStaticKey()) {} - UpsampleTrajectoryProfile::UpsampleTrajectoryProfile(double longest_valid_segment_length) - : Profile(UpsampleTrajectoryProfile::getStaticKey()), longest_valid_segment_length(longest_valid_segment_length) -{ -} - -std::size_t UpsampleTrajectoryProfile::getStaticKey() + : longest_valid_segment_length(longest_valid_segment_length) { - return std::type_index(typeid(UpsampleTrajectoryProfile)).hash_code(); } template