From ca1f571b7e6686342240eea7b44c3b4becbd573d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 4 Dec 2024 14:44:59 -0600 Subject: [PATCH 1/2] Add profile base class and update profile dictionary to not leverage template methods --- tesseract_command_language/CMakeLists.txt | 1 + .../include/tesseract_command_language/fwd.h | 1 + .../tesseract_command_language/profile.h | 68 +++++++ .../profile_dictionary.h | 176 ++++-------------- tesseract_command_language/src/profile.cpp | 46 +++++ .../src/profile_dictionary.cpp | 116 +++++++++++- .../src/basic_cartesian_example.cpp | 13 +- tesseract_examples/src/car_seat_example.cpp | 13 +- .../src/freespace_hybrid_example.cpp | 5 +- .../src/freespace_ompl_example.cpp | 2 +- .../src/glass_upright_example.cpp | 8 +- .../src/pick_and_place_example.cpp | 19 +- .../puzzle_piece_auxillary_axes_example.cpp | 15 +- .../src/puzzle_piece_example.cpp | 15 +- tesseract_motion_planners/core/CMakeLists.txt | 5 +- .../tesseract_motion_planners/planner_utils.h | 15 +- .../core/test/profile_dictionary_tests.cpp | 73 +++++--- .../descartes/CMakeLists.txt | 4 +- .../profile/descartes_default_plan_profile.h | 8 + .../descartes/profile/descartes_profile.h | 26 ++- .../descartes_default_plan_profile.cpp | 15 ++ .../src/profile/descartes_profile.cpp | 45 +++++ .../test/descartes_planner_tests.cpp | 6 +- tesseract_motion_planners/ompl/CMakeLists.txt | 1 + .../ompl/profile/ompl_default_plan_profile.h | 7 + .../ompl/profile/ompl_profile.h | 27 ++- .../src/profile/ompl_default_plan_profile.cpp | 12 ++ .../ompl/src/profile/ompl_profile.cpp | 48 +++++ .../ompl/test/ompl_planner_tests.cpp | 6 +- .../simple/CMakeLists.txt | 1 + ...e_planner_fixed_size_assign_plan_profile.h | 7 + .../simple_planner_fixed_size_plan_profile.h | 7 + .../simple_planner_lvs_no_ik_plan_profile.h | 7 + .../profile/simple_planner_lvs_plan_profile.h | 7 + .../simple/profile/simple_planner_profile.h | 50 +++-- .../simple/src/interpolation.cpp | 5 +- ...planner_fixed_size_assign_plan_profile.cpp | 13 ++ ...simple_planner_fixed_size_plan_profile.cpp | 13 ++ .../simple_planner_lvs_no_ik_plan_profile.cpp | 13 ++ .../simple_planner_lvs_plan_profile.cpp | 13 ++ .../src/profile/simple_planner_profile.cpp | 67 +++++++ .../trajopt/CMakeLists.txt | 1 + .../trajopt_default_composite_profile.h | 6 + .../profile/trajopt_default_plan_profile.h | 6 + .../profile/trajopt_default_solver_profile.h | 8 + .../trajopt/profile/trajopt_profile.h | 77 +++++--- .../trajopt_default_composite_profile.cpp | 12 ++ .../profile/trajopt_default_plan_profile.cpp | 12 ++ .../trajopt_default_solver_profile.cpp | 12 ++ .../trajopt/src/profile/trajopt_profile.cpp | 74 ++++++++ .../trajopt/test/trajopt_planner_tests.cpp | 32 ++-- .../trajopt_ifopt/CMakeLists.txt | 1 + .../trajopt_ifopt_default_composite_profile.h | 7 + .../trajopt_ifopt_default_plan_profile.h | 7 + .../trajopt_ifopt_default_solver_profile.h | 8 + .../profile/trajopt_ifopt_profile.h | 79 +++++--- ...rajopt_ifopt_default_composite_profile.cpp | 12 ++ .../trajopt_ifopt_default_plan_profile.cpp | 12 ++ .../trajopt_ifopt_default_solver_profile.cpp | 12 ++ .../src/profile/trajopt_ifopt_profile.cpp | 80 ++++++++ .../planning/CMakeLists.txt | 11 +- .../planning/profiles/contact_check_profile.h | 17 +- .../profiles/fix_state_bounds_profile.h | 19 +- .../profiles/fix_state_collision_profile.h | 23 ++- ...terative_spline_parameterization_profile.h | 25 ++- .../planning/profiles/min_length_profile.h | 21 ++- .../profiles/profile_switch_profile.h | 21 ++- .../ruckig_trajectory_smoothing_profile.h | 43 +++-- .../time_optimal_parameterization_profile.h | 29 ++- .../profiles/upsample_trajectory_profile.h | 24 ++- .../planning/src/nodes/min_length_task.cpp | 5 +- .../src/profiles/contact_check_profile.cpp | 17 ++ .../src/profiles/fix_state_bounds_profile.cpp | 49 +++++ .../profiles/fix_state_collision_profile.cpp | 56 ++++++ ...rative_spline_parameterization_profile.cpp | 62 ++++++ .../src/profiles/min_length_profile.cpp | 52 ++++++ .../src/profiles/profile_switch_profile.cpp | 49 +++++ .../ruckig_trajectory_smoothing_profile.cpp | 82 ++++++++ .../time_optimal_parameterization_profile.cpp | 69 +++++++ .../profiles/upsample_trajectory_profile.cpp | 55 ++++++ .../test/fix_state_bounds_task_unit.cpp | 2 +- .../tesseract_task_composer_planning_unit.cpp | 6 +- 82 files changed, 1804 insertions(+), 390 deletions(-) create mode 100644 tesseract_command_language/include/tesseract_command_language/profile.h create mode 100644 tesseract_command_language/src/profile.cpp create mode 100644 tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp create mode 100644 tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp create mode 100644 tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp create mode 100644 tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp create mode 100644 tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/min_length_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp diff --git a/tesseract_command_language/CMakeLists.txt b/tesseract_command_language/CMakeLists.txt index b2db2633c0e..80d199d2f9f 100644 --- a/tesseract_command_language/CMakeLists.txt +++ b/tesseract_command_language/CMakeLists.txt @@ -49,6 +49,7 @@ add_library( src/poly/waypoint_poly.cpp src/move_instruction.cpp src/profile_dictionary.cpp + src/profile.cpp src/set_analog_instruction.cpp src/set_tool_instruction.cpp src/timer_instruction.cpp diff --git a/tesseract_command_language/include/tesseract_command_language/fwd.h b/tesseract_command_language/include/tesseract_command_language/fwd.h index 16fdaad265b..c800a38f02b 100644 --- a/tesseract_command_language/include/tesseract_command_language/fwd.h +++ b/tesseract_command_language/include/tesseract_command_language/fwd.h @@ -31,6 +31,7 @@ enum class WaitInstructionType : int; class WaitInstruction; // Profile Dictionary +class Profile; class ProfileDictionary; } // namespace tesseract_planning #endif // TESSERACT_COMMAND_LANGUAGE_FWD_H diff --git a/tesseract_command_language/include/tesseract_command_language/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h new file mode 100644 index 00000000000..6d73f9de740 --- /dev/null +++ b/tesseract_command_language/include/tesseract_command_language/profile.h @@ -0,0 +1,68 @@ +/** + * @file profile.h + * @brief This is a profile base class + * + * @author Levi Armstrong + * @date December 2, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_PROFILE_H + +#include +#include +#include + +namespace tesseract_planning +{ +/** + * @brief The Profile class + */ +class Profile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + Profile() = default; + Profile(std::size_t id); + 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 +}; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::Profile) + +#endif // TESSERACT_MOTION_PLANNERS_PROFILE_H 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 0e3fb693b5a..d22194faa75 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -28,14 +28,12 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include namespace tesseract_planning @@ -50,13 +48,8 @@ namespace tesseract_planning using ProfileRemapping = std::unordered_map>; /** - * @brief This class is used to store profiles for motion planning and process planning + * @brief This class is used to store profiles used by various tasks * @details This is a thread safe class - * A ProfileEntry is a std::unordered_map> - * - The key is the profile name - * - Where std::shared_ptr is the profile - * The ProfleEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) - * @note When adding a profile entry the T should be the base class type. */ class ProfileDictionary { @@ -64,55 +57,6 @@ class ProfileDictionary using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - /** - * @brief Check if a profile entry exists - * @param ns The namesspace to search under - * @return True if exists, otherwise false - */ - template - bool hasProfileEntry(const std::string& ns) const - { - std::shared_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return false; - - return (it->second.find(std::type_index(typeid(ProfileType))) != it->second.end()); - } - - /** @brief Remove a profile entry */ - template - void removeProfileEntry(const std::string& ns) - { - std::unique_lock lock(mutex_); - - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return; - - it->second.erase(std::type_index(typeid(ProfileType))); - } - - /** - * @brief Get a profile entry - * @return The profile map associated with the profile entry - */ - template - std::unordered_map> getProfileEntry(const std::string& ns) 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(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - return std::any_cast>&>(it2->second); - - throw std::runtime_error("Profile entry does not exist for type name '" + - std::string(std::type_index(typeid(ProfileType)).name()) + "' in namespace '" + ns + "'!"); - } - /** * @brief Add a profile * @details If the profile entry does not exist it will create one @@ -120,114 +64,72 @@ class ProfileDictionary * @param profile_name The profile name * @param profile The profile to add */ - template - void addProfile(const std::string& ns, const std::string& profile_name, std::shared_ptr profile) - { - if (ns.empty()) - throw std::runtime_error("Adding profile with an empty namespace!"); - - if (profile_name.empty()) - throw std::runtime_error("Adding profile with an empty string as the key!"); - - if (profile == nullptr) - throw std::runtime_error("Adding profile that is a nullptr"); - - std::unique_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - { - std::unordered_map> new_entry; - new_entry[profile_name] = profile; - profiles_[ns][std::type_index(typeid(ProfileType))] = new_entry; - } - else - { - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - { - std::any_cast>&>(it2->second)[profile_name] = - profile; - } - else - { - std::unordered_map> new_entry; - new_entry[profile_name] = profile; - it->second[std::type_index(typeid(ProfileType))] = new_entry; - } - } - } + void addProfile(const std::string& ns, const std::string& profile_name, Profile::ConstPtr profile); /** * @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 */ - template - bool hasProfile(const std::string& ns, const std::string& profile_name) const - { - std::shared_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return false; - - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - { - const auto& profile_map = - std::any_cast>&>(it2->second); - auto it3 = profile_map.find(profile_name); - if (it3 != profile_map.end()) - return true; - } - return false; - } + bool hasProfile(std::size_t key, 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 */ - template - std::shared_ptr getProfile(const std::string& ns, const std::string& profile_name) const - { - std::shared_lock lock(mutex_); - const auto& it = profiles_.at(ns); - const auto& it2 = it.at(std::type_index(typeid(ProfileType))); - const auto& profile_map = - std::any_cast>&>(it2); - return profile_map.at(profile_name); - } + Profile::ConstPtr getProfile(std::size_t key, 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 */ - template - void removeProfile(const std::string& ns, const std::string& profile_name) - { - std::unique_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return; - - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - std::any_cast>&>(it2->second) - .erase(profile_name); - } + void removeProfile(std::size_t key, 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; + + /** + * @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); + + /** + * @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; /** @brief Clear the dictionary */ void clear(); protected: - std::unordered_map> profiles_; mutable std::shared_mutex mutex_; + std::unordered_map>> + profiles_; friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT }; + } // namespace tesseract_planning BOOST_CLASS_EXPORT_KEY(tesseract_planning::ProfileDictionary) diff --git a/tesseract_command_language/src/profile.cpp b/tesseract_command_language/src/profile.cpp new file mode 100644 index 00000000000..52f1eee0b6e --- /dev/null +++ b/tesseract_command_language/src/profile.cpp @@ -0,0 +1,46 @@ +/** + * @file profile.cpp + * @brief This is a profile base class + * + * @author Levi Armstrong + * @date December 2, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +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*/) +{ + ar& boost::serialization::make_nvp("key", key_); +} +} // namespace tesseract_planning + +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::Profile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::Profile) diff --git a/tesseract_command_language/src/profile_dictionary.cpp b/tesseract_command_language/src/profile_dictionary.cpp index 50657397a93..e788657905b 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -25,19 +25,131 @@ */ #include +#include #include +#include namespace tesseract_planning { +bool ProfileDictionary::hasProfileEntry(std::size_t key, 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()); +} + +void ProfileDictionary::removeProfileEntry(std::size_t key, const std::string& ns) +{ + const std::unique_lock lock(mutex_); + + auto it = profiles_.find(ns); + if (it == profiles_.end()) + return; + + it->second.erase(key); +} + +std::unordered_map ProfileDictionary::getProfileEntry(std::size_t key, + 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 + "'!"); +} + +void ProfileDictionary::addProfile(const std::string& ns, const std::string& profile_name, Profile::ConstPtr profile) +{ + if (ns.empty()) + throw std::runtime_error("Adding profile with an empty namespace!"); + + if (profile_name.empty()) + throw std::runtime_error("Adding profile with an empty string as the key!"); + + if (profile == nullptr) + throw std::runtime_error("Adding profile that is a nullptr"); + + const std::unique_lock lock(mutex_); + auto it = profiles_.find(ns); + if (it == profiles_.end()) + { + std::unordered_map new_entry; + new_entry[profile_name] = profile; + profiles_[ns][profile->getKey()] = 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; + } + } +} + +bool ProfileDictionary::hasProfile(std::size_t key, 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); + if (it2 != it->second.end()) + { + auto it3 = it2->second.find(profile_name); + if (it3 != it2->second.end()) + return true; + } + return false; +} + +Profile::ConstPtr ProfileDictionary::getProfile(std::size_t key, + 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); +} + +void ProfileDictionary::removeProfile(std::size_t key, 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); +} + void ProfileDictionary::clear() { - std::unique_lock lock(mutex_); + const std::unique_lock lock(mutex_); profiles_.clear(); } template -void ProfileDictionary::serialize(Archive& /*ar*/, const unsigned int /*version*/) +void ProfileDictionary::serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("profiles", profiles_); } } // namespace tesseract_planning diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 2ff9517c4f0..d39d6f7df56 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -232,14 +232,13 @@ bool BasicCartesianExample::run() composite_profile->smooth_accelerations = false; composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); } else { @@ -250,7 +249,7 @@ bool BasicCartesianExample::run() composite_profile->smooth_accelerations = false; composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_cost_config.enabled = false; @@ -259,8 +258,8 @@ bool BasicCartesianExample::run() plan_profile->joint_cost_config.enabled = false; plan_profile->joint_constraint_config.enabled = true; plan_profile->joint_constraint_config.coeff = Eigen::VectorXd::Ones(7); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); } // Create task diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index d81eedcd115..ade2c1d4e03 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -311,12 +311,9 @@ bool CarSeatExample::run() trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_solver_profile); } else { @@ -335,8 +332,8 @@ bool CarSeatExample::run() trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); } // Solve Trajectory diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index caafb555133..c674c79d837 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -203,7 +203,7 @@ bool FreespaceHybridExample::run() ompl_profile->planning_time = planning_time_; ompl_profile->planners = { ompl_planner_config, ompl_planner_config }; - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); if (ifopt_) { @@ -217,8 +217,7 @@ bool FreespaceHybridExample::run() trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); } // Create task diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 6db760c4387..199407c2ab2 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -188,7 +188,7 @@ bool FreespaceOMPLExample::run() // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); // Create task TaskComposerNode::UPtr task = factory.createTaskComposerNode("OMPLPipeline"); diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index fe1dbb2714f..5b0dfe88b17 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -211,7 +211,7 @@ bool GlassUprightExample::run() composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); auto plan_profile = std::make_shared(); plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); @@ -219,7 +219,7 @@ bool GlassUprightExample::run() plan_profile->cartesian_coeff(0) = 0; plan_profile->cartesian_coeff(1) = 0; plan_profile->cartesian_coeff(2) = 0; - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); } else { @@ -239,7 +239,7 @@ bool GlassUprightExample::run() composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_cost_config.enabled = false; @@ -250,7 +250,7 @@ bool GlassUprightExample::run() plan_profile->cartesian_constraint_config.coeff(2) = 0; // Add profile to Dictionary - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); } // Create task diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index ba05e644c91..1e013eafda2 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -258,12 +258,9 @@ bool PickAndPlaceExample::run() auto trajopt_ifopt_solver_profile = std::make_shared(); trajopt_ifopt_solver_profile->opt_info.max_iterations = 100; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { @@ -286,15 +283,15 @@ bool PickAndPlaceExample::run() auto trajopt_solver_profile = std::make_shared(); trajopt_solver_profile->opt_info.max_iter = 100; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); } auto post_check_profile = std::make_shared(); - profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile); - profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile); + profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile); + profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile); CONSOLE_BRIDGE_logInform("Pick plan"); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index 7e71909b52b..3ab5620abd8 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -257,12 +257,9 @@ bool PuzzlePieceAuxillaryAxesExample::run() trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { @@ -291,9 +288,9 @@ bool PuzzlePieceAuxillaryAxesExample::run() trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); } // Create task diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 730e53ce3a7..94a1e1fea31 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -247,12 +247,9 @@ bool PuzzlePieceExample::run() trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { @@ -277,9 +274,9 @@ bool PuzzlePieceExample::run() trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); } // Create task diff --git a/tesseract_motion_planners/core/CMakeLists.txt b/tesseract_motion_planners/core/CMakeLists.txt index 1ee03a59fe1..949263a39a2 100644 --- a/tesseract_motion_planners/core/CMakeLists.txt +++ b/tesseract_motion_planners/core/CMakeLists.txt @@ -1,5 +1,8 @@ # 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/core/include/tesseract_motion_planners/planner_utils.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h index ac8fbe45785..9d0eb86da14 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,8 +138,9 @@ std::shared_ptr getProfile(const std::string& ns, const ProfileDictionary& profile_dictionary, std::shared_ptr default_profile = nullptr) { - if (profile_dictionary.hasProfile(ns, profile)) - return profile_dictionary.getProfile(ns, profile); + if (profile_dictionary.hasProfile(ProfileType::getStaticKey(), ns, profile)) + return std::static_pointer_cast( + profile_dictionary.getProfile(ProfileType::getStaticKey(), ns, profile)); CONSOLE_BRIDGE_logDebug("Profile '%s' was not found in namespace '%s' for type '%s'. Using default if available. " "Available " @@ -148,9 +149,9 @@ std::shared_ptr getProfile(const std::string& ns, ns.c_str(), typeid(ProfileType).name()); - if (profile_dictionary.hasProfileEntry(ns)) + if (profile_dictionary.hasProfileEntry(ProfileType::getStaticKey(), ns)) { - for (const auto& pair : profile_dictionary.getProfileEntry(ns)) + for (const auto& pair : profile_dictionary.getProfileEntry(ProfileType::getStaticKey(), ns)) { CONSOLE_BRIDGE_logDebug("%s", pair.first.c_str()); } @@ -171,14 +172,14 @@ std::shared_ptr getProfile(const std::string& ns, template std::shared_ptr applyProfileOverrides(const std::string& ns, const std::string& profile, - const std::shared_ptr& nominal_profile, + const std::shared_ptr& nominal_profile, const ProfileDictionary::ConstPtr& overrides = nullptr) { if (!overrides) return nominal_profile; - if (overrides->hasProfile(ns, profile)) - return overrides->getProfile(ns, profile); + if (overrides->hasProfile(ProfileType::getStaticKey(), ns, profile)) + return std::static_pointer_cast(overrides->getProfile(ProfileType::getStaticKey(), 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 5159c79055e..ea0c4cbe6b6 100644 --- a/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp @@ -29,9 +29,18 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include -struct ProfileBase +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 }; }; @@ -41,8 +50,16 @@ struct ProfileTest : public ProfileBase ProfileTest(int a) { this->a = a; } }; -struct ProfileBase2 +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 }; }; @@ -58,69 +75,69 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT { ProfileDictionary profiles; - EXPECT_FALSE(profiles.hasProfileEntry("ns")); + EXPECT_FALSE(profiles.hasProfileEntry(ProfileBase::getStaticKey(), "ns")); - profiles.addProfile("ns", "key", std::make_shared()); + profiles.addProfile("ns", "key", std::make_shared()); - EXPECT_TRUE(profiles.hasProfileEntry("ns")); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); + EXPECT_TRUE(profiles.hasProfileEntry(ProfileBase::getStaticKey(), "ns")); + EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "ns", "key")); - auto profile = profiles.getProfile("ns", "key"); + auto profile = getProfile("ns", "key", profiles); EXPECT_TRUE(profile != nullptr); EXPECT_EQ(profile->a, 0); // Check add same profile with different key - profiles.addProfile("ns", "key2", profile); - EXPECT_TRUE(profiles.hasProfile("ns", "key2")); - auto profile2 = profiles.getProfile("ns", "key2"); + profiles.addProfile("ns", "key2", profile); + EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "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("ns", "key")); - auto profile_check = profiles.getProfile("ns", "key"); + profiles.addProfile("ns", "key", std::make_shared(10)); + EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "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("ns"); + auto profile_map = profiles.getProfileEntry(ProfileBase::getStaticKey(), "ns"); auto it = profile_map.find("key"); EXPECT_TRUE(it != profile_map.end()); - EXPECT_EQ(it->second->a, 10); + EXPECT_EQ(std::static_pointer_cast(it->second)->a, 10); - profiles.addProfile("ns", "key", std::make_shared(20)); - auto profile_check2 = profiles.getProfile("ns", "key"); + profiles.addProfile("ns", "key", std::make_shared(20)); + auto profile_check2 = getProfile("ns", "key", profiles); EXPECT_TRUE(profile_check2 != nullptr); EXPECT_EQ(profile_check2->a, 20); // Request a profile entry namespace that does not exist - EXPECT_ANY_THROW(profiles.getProfileEntry("DoesNotExist")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfileEntry(0, "DoesNotExist")); // NOLINT // Request a profile that does not exist - EXPECT_ANY_THROW(profiles.getProfile("DoesNotExist", "key")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile(ProfileBase::getStaticKey(), "DoesNotExist", "key")); // NOLINT // Request a profile that does not exist - EXPECT_ANY_THROW(profiles.getProfile("ns", "DoesNotExist")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile(ProfileBase::getStaticKey(), "ns", "DoesNotExist")); // NOLINT // Check adding a empty namespace - EXPECT_ANY_THROW(profiles.addProfile("", "key3", std::make_shared(20))); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("", "key3", std::make_shared(20))); // NOLINT // Check adding a empty key - EXPECT_ANY_THROW(profiles.addProfile("ns", "", std::make_shared(20))); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("ns", "", std::make_shared(20))); // NOLINT // Check adding a nullptr profile - EXPECT_ANY_THROW(profiles.addProfile("ns", "key", nullptr)); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("ns", "key", nullptr)); // NOLINT // Add different profile entry - profiles.addProfile("ns", "key", std::make_shared(5)); - EXPECT_TRUE(profiles.hasProfileEntry("ns")); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); - auto profile_check3 = profiles.getProfile("ns", "key"); + profiles.addProfile("ns", "key", std::make_shared(5)); + EXPECT_TRUE(profiles.hasProfileEntry(ProfileBase2::getStaticKey(), "ns")); + EXPECT_TRUE(profiles.hasProfile(ProfileBase2::getStaticKey(), "ns", "key")); + auto profile_check3 = getProfile("ns", "key", profiles); EXPECT_TRUE(profile_check3 != nullptr); EXPECT_EQ(profile_check3->b, 5); // Check that other profile entry with same key is not affected - auto profile_check4 = profiles.getProfile("ns", "key"); + auto profile_check4 = getProfile("ns", "key", profiles); EXPECT_TRUE(profile_check4 != nullptr); EXPECT_EQ(profile_check4->a, 20); } diff --git a/tesseract_motion_planners/descartes/CMakeLists.txt b/tesseract_motion_planners/descartes/CMakeLists.txt index e7626c99338..097b694dcfb 100644 --- a/tesseract_motion_planners/descartes/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/CMakeLists.txt @@ -11,7 +11,9 @@ add_library( src/serialize.cpp src/deserialize.cpp src/descartes_utils.cpp - src/profile/descartes_default_plan_profile.cpp) + src/profile/descartes_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 63460a66659..a52bdc9ebad 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 @@ -103,10 +103,18 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile int index) const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; using DescartesDefaultPlanProfileF = DescartesDefaultPlanProfile; using DescartesDefaultPlanProfileD = DescartesDefaultPlanProfile; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesDefaultPlanProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_DEFAULT_PLAN_PROFILE_H 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 e6a02fe316e..b880b75f8d9 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,11 +29,13 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include namespace tinyxml2 @@ -45,18 +47,23 @@ class XMLDocument; namespace tesseract_planning { template -class DescartesPlanProfile +class DescartesPlanProfile : public Profile { public: using Ptr = std::shared_ptr>; using ConstPtr = std::shared_ptr>; - DescartesPlanProfile() = default; - virtual ~DescartesPlanProfile() = default; - DescartesPlanProfile(const DescartesPlanProfile&) = default; - DescartesPlanProfile& operator=(const DescartesPlanProfile&) = default; - DescartesPlanProfile(DescartesPlanProfile&&) noexcept = default; - DescartesPlanProfile& operator=(DescartesPlanProfile&&) noexcept = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey() { return std::type_index(typeid(DescartesPlanProfile)).hash_code(); } virtual void apply(DescartesProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, @@ -71,6 +78,11 @@ class DescartesPlanProfile int index) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int); // NOLINT }; /** @todo Currently descartes does not have support of composite profile everything is handled by the plan profile */ 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 af0b93125e8..f73cc5d87d4 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,6 +24,8 @@ * limitations under the License. */ #include +#include +#include namespace tesseract_planning { @@ -31,4 +33,17 @@ namespace tesseract_planning template class DescartesDefaultPlanProfile; template class DescartesDefaultPlanProfile; +template +template +void DescartesDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DescartesPlanProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesDefaultPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesDefaultPlanProfile) diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp new file mode 100644 index 00000000000..0eeffa0a126 --- /dev/null +++ b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp @@ -0,0 +1,45 @@ +/** + * @file descartes_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include + +namespace tesseract_planning +{ +template +template +void DescartesPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) diff --git a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp index b8dc02bd2f1..df74d6dccd9 100644 --- a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp @@ -145,7 +145,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); @@ -261,7 +261,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); @@ -364,7 +364,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planning Request PlannerRequest request; diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index cac76bc1ec2..ae481465b31 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -10,6 +10,7 @@ set(OMPL_SRC src/weighted_real_vector_state_sampler.cpp src/ompl_planner_configurator.cpp src/ompl_problem.cpp + src/profile/ompl_profile.cpp src/profile/ompl_default_plan_profile.cpp src/utils.cpp src/state_collision_validator.cpp 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 b65cfbef016..fbb7e9f4c71 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 @@ -160,6 +160,13 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile void processMotionValidator(OMPLProblem& prob, const ompl::base::StateValidityCheckerPtr& svc_without_collision) const; void processOptimizationObjective(OMPLProblem& prob) const; + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H 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 1e64f17c4af..f02e779f548 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 @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tinyxml2 { @@ -45,20 +46,23 @@ class XMLDocument; namespace tesseract_planning { struct OMPLProblem; -class OMPLPlanProfile +class OMPLPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - OMPLPlanProfile() = default; - virtual ~OMPLPlanProfile() = default; - OMPLPlanProfile(const OMPLPlanProfile&) = default; - OMPLPlanProfile& operator=(const OMPLPlanProfile&) = default; - OMPLPlanProfile(OMPLPlanProfile&&) noexcept = default; - OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = default; + OMPLPlanProfile(); + OMPLPlanProfile(const OMPLPlanProfile&) = delete; + OMPLPlanProfile& operator=(const OMPLPlanProfile&) = delete; + OMPLPlanProfile(OMPLPlanProfile&&) noexcept = delete; + OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = delete; - OMPLPlanProfile(const tinyxml2::XMLElement& xml_element); + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void setup(OMPLProblem& prob) const = 0; @@ -91,10 +95,17 @@ class OMPLPlanProfile int index) 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 }; /** @todo Currently OMPL does not have support of composite profile everything is handled by the plan profile */ } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROFILE_H 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 9002d1b27c0..08231b7275d 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 @@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -777,4 +779,14 @@ void OMPLDefaultPlanProfile::processOptimizationObjective(OMPLProblem& prob) con } } +template +void OMPLDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlanProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLDefaultPlanProfile) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp new file mode 100644 index 00000000000..2763116d0bd --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp @@ -0,0 +1,48 @@ +/** + * @file ompl_profile.cpp + * @brief Tesseract OMPL profile + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLPlanProfile) diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 09e771152b2..45052b5b2b2 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -242,7 +242,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner Request PlannerRequest request; @@ -406,7 +406,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner Request PlannerRequest request; @@ -501,7 +501,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner Request PlannerRequest request; diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 22d2c2fc3f8..0b630202360 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -2,6 +2,7 @@ add_library( ${PROJECT_NAME}_simple src/interpolation.cpp src/simple_motion_planner.cpp + src/profile/simple_planner_profile.cpp src/profile/simple_planner_lvs_plan_profile.cpp src/profile/simple_planner_lvs_no_ik_plan_profile.cpp src/profile/simple_planner_fixed_size_assign_plan_profile.cpp diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h index 81ae271d7de..dbef4fa0410 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h @@ -56,8 +56,15 @@ class SimplePlannerFixedSizeAssignPlanProfile : public SimplePlannerPlanProfile /** @brief The number of steps to use for linear instruction */ int linear_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h index 83f8ba0907b..341ef430db7 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h @@ -55,8 +55,15 @@ class SimplePlannerFixedSizePlanProfile : public SimplePlannerPlanProfile /** @brief The number of steps to use for linear instruction */ int linear_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerFixedSizePlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h index 5bc86a9a88c..412d550bff8 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h @@ -78,8 +78,15 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile /** @brief The maximum number of steps for the plan */ int max_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSNoIKPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_LVS_NO_IK_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h index 02725e739a5..8fa90c9619d 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h @@ -77,8 +77,15 @@ class SimplePlannerLVSPlanProfile : public SimplePlannerPlanProfile /** @brief The maximum number of steps for the plan */ int max_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_LVS_PLAN_PROFILE_H 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 d265b3d9b1d..fe4ff859739 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 @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_planning { @@ -42,18 +43,23 @@ namespace tesseract_planning * @brief Plan Profile for the simple planner. It defines some functions that handle each of the waypoint cases. The * planner then simply loops over all of the plan instructions and calls the correct function */ -class SimplePlannerPlanProfile +class SimplePlannerPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SimplePlannerPlanProfile() = default; - virtual ~SimplePlannerPlanProfile() = default; - SimplePlannerPlanProfile(const SimplePlannerPlanProfile&) = default; - SimplePlannerPlanProfile& operator=(const SimplePlannerPlanProfile&) = default; - SimplePlannerPlanProfile(SimplePlannerPlanProfile&&) noexcept = default; - SimplePlannerPlanProfile& operator=(SimplePlannerPlanProfile&&) noexcept = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** * @brief Generate a seed for the provided base_instruction @@ -73,24 +79,40 @@ class SimplePlannerPlanProfile const InstructionPoly& next_instruction, const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; -class SimplePlannerCompositeProfile +class SimplePlannerCompositeProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SimplePlannerCompositeProfile() = default; - virtual ~SimplePlannerCompositeProfile() = default; - SimplePlannerCompositeProfile(const SimplePlannerCompositeProfile&) = default; - SimplePlannerCompositeProfile& operator=(const SimplePlannerCompositeProfile&) = default; - SimplePlannerCompositeProfile(SimplePlannerCompositeProfile&&) noexcept = default; - SimplePlannerCompositeProfile& operator=(SimplePlannerCompositeProfile&&) noexcept = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); // This contains functions for composite processing. Get start for example +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PROFILE_H diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 73836099acd..850db0d7caf 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -1348,11 +1348,10 @@ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& ins // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(planner.getName(), instructions.getProfile(), profile); + profiles->addProfile(planner.getName(), instructions.getProfile(), profile); auto flat = instructions.flatten(&moveFilter); for (const auto& i : flat) - profiles->addProfile( - planner.getName(), i.get().as().getProfile(), profile); + profiles->addProfile(planner.getName(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = profiles; 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 4adc288996b..3356d09a848 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 @@ -36,6 +36,9 @@ #include +#include +#include + namespace tesseract_planning { SimplePlannerFixedSizeAssignPlanProfile::SimplePlannerFixedSizeAssignPlanProfile(int freespace_steps, int linear_steps) @@ -124,4 +127,14 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre return getInterpolatedInstructions(info2.manip->getJointNames(), states, info2.instruction); } +template +void SimplePlannerFixedSizeAssignPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) 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 8857bc7bac2..11c83b703e8 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 @@ -33,6 +33,9 @@ #include +#include +#include + namespace tesseract_planning { SimplePlannerFixedSizePlanProfile::SimplePlannerFixedSizePlanProfile(int freespace_steps, int linear_steps) @@ -63,4 +66,14 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env_state); } +template +void SimplePlannerFixedSizePlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizePlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizePlanProfile) 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 ccbf1fd781c..be4a21576c4 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 @@ -33,6 +33,9 @@ #include +#include +#include + namespace tesseract_planning { SimplePlannerLVSNoIKPlanProfile::SimplePlannerLVSNoIKPlanProfile(double state_longest_valid_segment_length, @@ -96,4 +99,14 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru request.env_state); } +template +void SimplePlannerLVSNoIKPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerLVSNoIKPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerLVSNoIKPlanProfile) 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 5ba45d76f26..c38a28264f8 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 @@ -33,6 +33,9 @@ #include +#include +#include + namespace tesseract_planning { SimplePlannerLVSPlanProfile::SimplePlannerLVSPlanProfile(double state_longest_valid_segment_length, @@ -96,4 +99,14 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio request.env_state); } +template +void SimplePlannerLVSPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerLVSPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerLVSPlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp new file mode 100644 index 00000000000..fa055489a01 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp @@ -0,0 +1,67 @@ +/** + * @file simple_planner_profile.cpp + * @brief + * + * @author Matthew Powelson + * @date July 23, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#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*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerCompositeProfile) diff --git a/tesseract_motion_planners/trajopt/CMakeLists.txt b/tesseract_motion_planners/trajopt/CMakeLists.txt index 5ac51385590..cd8c92b8847 100644 --- a/tesseract_motion_planners/trajopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt/CMakeLists.txt @@ -8,6 +8,7 @@ add_library( src/trajopt_waypoint_config.cpp src/trajopt_motion_planner.cpp src/trajopt_utils.cpp + src/profile/trajopt_profile.cpp src/profile/trajopt_default_plan_profile.cpp src/profile/trajopt_default_composite_profile.cpp src/profile/trajopt_default_solver_profile.cpp 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 14a17c18cae..a45debb90fb 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 @@ -153,7 +153,13 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile bool& enabled, Eigen::VectorXd& coeff, std::size_t& length); + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptDefaultCompositeProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_COMPOSITE_PROFILE_H 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 83bcdf96710..7da66147c04 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 @@ -98,7 +98,13 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci, int index) const; void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci, const std::vector& fixed_steps) const; + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_PLAN_PROFILE_H 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 04f0850ac29..908911a1361 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 @@ -60,6 +60,14 @@ class TrajOptDefaultSolverProfile : public TrajOptSolverProfile void apply(trajopt::ProblemConstructionInfo& pci) const override; 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::TrajOptDefaultSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_SOLVER_PROFILE_H 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 3bfe558b05a..f9e332bd107 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 @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tinyxml2 { @@ -44,18 +45,23 @@ class XMLDocument; namespace tesseract_planning { -class TrajOptPlanProfile +class TrajOptPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptPlanProfile() = default; - virtual ~TrajOptPlanProfile() = default; - TrajOptPlanProfile(const TrajOptPlanProfile&) = default; - TrajOptPlanProfile& operator=(const TrajOptPlanProfile&) = default; - TrajOptPlanProfile(TrajOptPlanProfile&&) = default; - TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(trajopt::ProblemConstructionInfo& pci, const CartesianWaypointPoly& cartesian_waypoint, @@ -72,20 +78,30 @@ class TrajOptPlanProfile int index) 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 }; -class TrajOptCompositeProfile +class TrajOptCompositeProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptCompositeProfile() = default; - virtual ~TrajOptCompositeProfile() = default; - TrajOptCompositeProfile(const TrajOptCompositeProfile&) = default; - TrajOptCompositeProfile& operator=(const TrajOptCompositeProfile&) = default; - TrajOptCompositeProfile(TrajOptCompositeProfile&&) = default; - TrajOptCompositeProfile& operator=(TrajOptCompositeProfile&&) = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(trajopt::ProblemConstructionInfo& pci, int start_index, @@ -95,26 +111,45 @@ class TrajOptCompositeProfile const std::vector& fixed_indices) 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 }; -class TrajOptSolverProfile +class TrajOptSolverProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptSolverProfile() = default; - virtual ~TrajOptSolverProfile() = default; - TrajOptSolverProfile(const TrajOptSolverProfile&) = default; - TrajOptSolverProfile& operator=(const TrajOptSolverProfile&) = default; - TrajOptSolverProfile(TrajOptSolverProfile&&) = default; - TrajOptSolverProfile& operator=(TrajOptSolverProfile&&) = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(trajopt::ProblemConstructionInfo& pci) 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 }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptPlanProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptCompositeProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_PROFILE_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 a4d5369e6ae..14f12308bd4 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 @@ -29,6 +29,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -463,4 +465,14 @@ void TrajOptDefaultCompositeProfile::addAvoidSingularity(trajopt::ProblemConstru pci.cost_infos.push_back(ti); } +template +void TrajOptDefaultCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptCompositeProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptDefaultCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptDefaultCompositeProfile) 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 e2cc8a29a64..039cc388b84 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 @@ -27,6 +27,8 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -311,4 +313,14 @@ tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& do return xml_planner; } + +template +void TrajOptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptPlanProfile); +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptDefaultPlanProfile) 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 0332baf2de0..b620a18ce0f 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 @@ -28,6 +28,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -44,4 +46,14 @@ void TrajOptDefaultSolverProfile::apply(trajopt::ProblemConstructionInfo& pci) c tinyxml2::XMLElement* TrajOptDefaultSolverProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const { return nullptr; } +template +void TrajOptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptSolverProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptDefaultSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptDefaultSolverProfile) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp new file mode 100644 index 00000000000..190c23d1e47 --- /dev/null +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp @@ -0,0 +1,74 @@ +/** + * @file trajopt_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#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*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptCompositeProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptSolverProfile) diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 446fa1ec105..2b6ad5310cf 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -151,8 +151,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -226,8 +226,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -313,8 +313,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -404,8 +404,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -495,8 +495,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -586,8 +586,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -677,8 +677,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -742,8 +742,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); diff --git a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt index 711c515ac29..7f03805c0b9 100644 --- a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt @@ -7,6 +7,7 @@ add_library( src/trajopt_ifopt_motion_planner.cpp src/trajopt_ifopt_problem.cpp src/trajopt_ifopt_utils.cpp + src/profile/trajopt_ifopt_profile.cpp src/profile/trajopt_ifopt_default_plan_profile.cpp src/profile/trajopt_ifopt_default_composite_profile.cpp src/profile/trajopt_ifopt_default_solver_profile.cpp) diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h index c4ca46dc05c..2d7fb81c96b 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h @@ -90,7 +90,14 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile const std::vector& fixed_indices) const override; 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::TrajOptIfoptDefaultCompositeProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h index 9676c365dd7..01264d5c1a3 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h @@ -65,7 +65,14 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile int index) const override; 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::TrajOptIfoptDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_TrajOptIfopt_IFOPT_DEFAULT_PLAN_PROFILE_H 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 80be4dbb703..4cb61a3a966 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 @@ -69,6 +69,14 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile void apply(TrajOptIfoptProblem& problem) const override; 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::TrajOptIfoptDefaultSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H 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 35b62e19826..55fc37eb31c 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 @@ -33,8 +33,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tinyxml2 { @@ -46,18 +47,23 @@ namespace tesseract_planning { struct TrajOptIfoptProblem; -class TrajOptIfoptPlanProfile +class TrajOptIfoptPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptPlanProfile() = default; - virtual ~TrajOptIfoptPlanProfile() = default; - TrajOptIfoptPlanProfile(const TrajOptIfoptPlanProfile&) = default; - TrajOptIfoptPlanProfile& operator=(const TrajOptIfoptPlanProfile&) = default; - TrajOptIfoptPlanProfile(TrajOptIfoptPlanProfile&&) = default; - TrajOptIfoptPlanProfile& operator=(TrajOptIfoptPlanProfile&&) = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(TrajOptIfoptProblem& problem, const CartesianWaypointPoly& cartesian_waypoint, @@ -74,20 +80,30 @@ class TrajOptIfoptPlanProfile int index) 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 }; -class TrajOptIfoptCompositeProfile +class TrajOptIfoptCompositeProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptCompositeProfile() = default; - virtual ~TrajOptIfoptCompositeProfile() = default; - TrajOptIfoptCompositeProfile(const TrajOptIfoptCompositeProfile&) = default; - TrajOptIfoptCompositeProfile& operator=(const TrajOptIfoptCompositeProfile&) = default; - TrajOptIfoptCompositeProfile(TrajOptIfoptCompositeProfile&&) = default; - TrajOptIfoptCompositeProfile& operator=(TrajOptIfoptCompositeProfile&&) = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(TrajOptIfoptProblem& problem, int start_index, @@ -97,26 +113,45 @@ class TrajOptIfoptCompositeProfile const std::vector& fixed_indices) 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 }; -class TrajOptIfoptSolverProfile +class TrajOptIfoptSolverProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptSolverProfile() = default; - virtual ~TrajOptIfoptSolverProfile() = default; - TrajOptIfoptSolverProfile(const TrajOptIfoptSolverProfile&) = default; - TrajOptIfoptSolverProfile& operator=(const TrajOptIfoptSolverProfile&) = default; - TrajOptIfoptSolverProfile(TrajOptIfoptSolverProfile&&) = default; - TrajOptIfoptSolverProfile& operator=(TrajOptIfoptSolverProfile&&) = default; + 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 + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(TrajOptIfoptProblem& problem) 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 }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptPlanProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptCompositeProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROFILE_H 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 290dc437d62..fc837f100cb 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,6 +29,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -97,4 +99,14 @@ tinyxml2::XMLElement* TrajOptIfoptDefaultCompositeProfile::toXML(tinyxml2::XMLDo throw std::runtime_error("TrajOptIfoptDefaultCompositeProfile::toXML is not implemented!"); } +template +void TrajOptIfoptDefaultCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptCompositeProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptDefaultCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptDefaultCompositeProfile) 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 4283be0dd5e..a2d6994a49a 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 @@ -29,6 +29,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -132,4 +134,14 @@ tinyxml2::XMLElement* TrajOptIfoptDefaultPlanProfile::toXML(tinyxml2::XMLDocumen throw std::runtime_error("TrajOptIfoptDefaultPlanProfile::toXML is not implemented!"); } +template +void TrajOptIfoptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptPlanProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptDefaultPlanProfile) 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 c7e149fa93c..fff3fd303e1 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 @@ -28,6 +28,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -62,4 +64,14 @@ tinyxml2::XMLElement* TrajOptIfoptDefaultSolverProfile::toXML(tinyxml2::XMLDocum throw std::runtime_error("TrajOptIfoptDefaultSolverProfile::toXML is not implemented!"); } +template +void TrajOptIfoptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptSolverProfile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptDefaultSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptDefaultSolverProfile) 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 new file mode 100644 index 00000000000..5f5f7dac869 --- /dev/null +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp @@ -0,0 +1,80 @@ +/** + * @file trajopt_ifopt_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#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*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptCompositeProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptSolverProfile) diff --git a/tesseract_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index 3fc1ce07ad7..6fd736e5fba 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -30,7 +30,16 @@ set(LIB_SOURCE_FILES src/nodes/upsample_trajectory_task.cpp src/nodes/raster_motion_task.cpp src/nodes/raster_only_motion_task.cpp - src/profiles/contact_check_profile.cpp) + src/profiles/contact_check_profile.cpp + src/profiles/fix_state_bounds_profile.cpp + src/profiles/fix_state_collision_profile.cpp + src/profiles/iterative_spline_parameterization_profile.cpp + src/profiles/min_length_profile.cpp + 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 + ) set(LIB_SOURCE_LINK_LIBRARIES ${PROJECT_NAME} 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 bf4dab5933d..c81f96e55b5 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 @@ -32,23 +32,34 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { -struct ContactCheckProfile +struct ContactCheckProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; ContactCheckProfile(); - ContactCheckProfile(double longest_valid_segment_length, double contact_distance); - virtual ~ContactCheckProfile() = default; + /** + * @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; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::ContactCheckProfile) + #endif // TESSERACT_TASK_COMPOSER_CONTACT_CHECK_PROFILE_H 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 f8e97dfd41c..2f85a566f87 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 @@ -32,9 +32,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct FixStateBoundsProfile +struct FixStateBoundsProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -47,7 +49,13 @@ struct FixStateBoundsProfile DISABLED }; - FixStateBoundsProfile(Settings mode = Settings::ALL) : mode(mode) {} + 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; @@ -60,7 +68,14 @@ struct FixStateBoundsProfile /** @brief Amount to increase the lower bounds before clamping limits. Should be > 1 */ double lower_bounds_reduction{ std::numeric_limits::epsilon() }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::FixStateBoundsProfile) + #endif // TESSERACT_TASK_COMPOSER_FIX_STATE_BOUNDS_PROFILE_H 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 41985894711..7f37d1536f4 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 @@ -33,10 +33,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { -struct FixStateCollisionProfile +struct FixStateCollisionProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -60,11 +61,13 @@ struct FixStateCollisionProfile RANDOM_SAMPLER }; - FixStateCollisionProfile(Settings mode = Settings::ALL) : mode(mode) - { - collision_check_config.contact_request.type = tesseract_collision::ContactTestType::FIRST; - collision_check_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - } + 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; @@ -81,6 +84,14 @@ struct FixStateCollisionProfile /** @brief Number of sampling attempts if TrajOpt correction fails*/ int sampling_attempts{ 100 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::FixStateCollisionProfile) + #endif // TESSERACT_TASK_COMPOSER_FIX_STATE_COLLISION_PROFILE_H 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 4d2477ebf37..dcb7bc4301f 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 @@ -31,26 +31,37 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct IterativeSplineParameterizationProfile +struct IterativeSplineParameterizationProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - IterativeSplineParameterizationProfile() = default; - IterativeSplineParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor) - : max_velocity_scaling_factor(max_velocity_scaling_factor) - , max_acceleration_scaling_factor(max_acceleration_scaling_factor) - { - } + IterativeSplineParameterizationProfile(); + 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 }; /** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */ double max_acceleration_scaling_factor{ 1.0 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::IterativeSplineParameterizationProfile) + #endif // TESSERACT_TASK_COMPOSER_ITERATIVE_SPLINE_PARAMETERIZATION_PROFILE_H 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 c2ccb301e04..adfd5d9ec8e 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 @@ -33,18 +33,33 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct MinLengthProfile +struct MinLengthProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - MinLengthProfile() = default; - MinLengthProfile(long min_length) : min_length(min_length) {} + MinLengthProfile(); + 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: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::MinLengthProfile) + #endif // TESSERACT_TASK_COMPOSER_MIN_LENGTH_PROFILE_H 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 5f8e5773b9d..e1b2addc780 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 @@ -29,17 +29,32 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct ProfileSwitchProfile +struct ProfileSwitchProfile : public Profile { - ProfileSwitchProfile(int return_value = 1) : return_value(return_value) {} - using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; + 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: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::ProfileSwitchProfile) + #endif // TESSERACT_TASK_COMPOSER_PROFILE_SWITCH_PROFILE_H 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 265dd5796a5..8b99bce65b3 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 @@ -29,24 +29,26 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct RuckigTrajectorySmoothingCompositeProfile +struct RuckigTrajectorySmoothingCompositeProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - RuckigTrajectorySmoothingCompositeProfile() = default; + RuckigTrajectorySmoothingCompositeProfile(); RuckigTrajectorySmoothingCompositeProfile(double duration_extension_fraction, double max_duration_extension_factor, double max_velocity_scaling_factor, - double max_acceleration_scaling_factor) - : 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) - { - } + 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,17 +66,19 @@ struct RuckigTrajectorySmoothingCompositeProfile double max_jerk_scaling_factor{ 1.0 }; }; -struct RuckigTrajectorySmoothingMoveProfile +struct RuckigTrajectorySmoothingMoveProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - RuckigTrajectorySmoothingMoveProfile() = default; - RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor) - : max_velocity_scaling_factor(max_velocity_scaling_factor) - , max_acceleration_scaling_factor(max_acceleration_scaling_factor) - { - } + RuckigTrajectorySmoothingMoveProfile(); + 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 }; @@ -84,7 +88,14 @@ struct RuckigTrajectorySmoothingMoveProfile /** @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 }; } // namespace tesseract_planning +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 22a1d40b4a8..afc29a36774 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 @@ -32,26 +32,27 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct TimeOptimalParameterizationProfile +struct TimeOptimalParameterizationProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TimeOptimalParameterizationProfile() = default; + TimeOptimalParameterizationProfile(); TimeOptimalParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, double max_jerk_scaling_factor, double path_tolerance, - double min_angle_change) - : 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) - , min_angle_change(min_angle_change) - { - } + 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 }; @@ -67,7 +68,15 @@ struct TimeOptimalParameterizationProfile /** @brief At least one joint must change by greater than this amount for the point to be added. Default: 0.001*/ double min_angle_change{ 0.001 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TimeOptimalParameterizationProfile) + #endif // TIME_OPTIMAL_PARAMETERIZATION_PROFILE_H 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 df940e7f9fa..6bd56752aea 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 @@ -30,21 +30,33 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct UpsampleTrajectoryProfile +struct UpsampleTrajectoryProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - UpsampleTrajectoryProfile() = default; - UpsampleTrajectoryProfile(double longest_valid_segment_length) - : longest_valid_segment_length(longest_valid_segment_length) - { - } + UpsampleTrajectoryProfile(); + 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: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::UpsampleTrajectoryProfile) + #endif // TESSERACT_TASK_COMPOSER_UPSAMPLE_TRAJECTORY_PROFILE_H diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index 5fe97069635..32a5a0fa4e6 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -156,11 +156,10 @@ std::unique_ptr MinLengthTask::runImpl(TaskComposerContext // Create profile dictionary auto simple_profiles = std::make_shared(); - simple_profiles->addProfile(planner.getName(), ci.getProfile(), profile); + simple_profiles->addProfile(planner.getName(), ci.getProfile(), profile); auto flat = ci.flatten(&moveFilter); for (const auto& i : flat) - simple_profiles->addProfile( - planner.getName(), i.get().as().getProfile(), profile); + simple_profiles->addProfile(planner.getName(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = simple_profiles; 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 e7a7c98bedb..1358f2f5015 100644 --- a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp @@ -27,6 +27,9 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -36,6 +39,7 @@ 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; @@ -49,4 +53,17 @@ ContactCheckProfile::ContactCheckProfile(double longest_valid_segment_length, do config.longest_valid_segment_length = 0.05; } } + +std::size_t ContactCheckProfile::getStaticKey() { return std::type_index(typeid(ContactCheckProfile)).hash_code(); } + +template +void ContactCheckProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ContactCheckProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ContactCheckProfile) 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 new file mode 100644 index 00000000000..82dc182168a --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp @@ -0,0 +1,49 @@ +/** + * @file fix_state_bounds_profile.cpp + * @brief Profile for process that pushes plan instructions back within joint limits + * + * @author Matthew Powelson + * @date August 31. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#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(); } + +template +void FixStateBoundsProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FixStateBoundsProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateBoundsProfile) 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 new file mode 100644 index 00000000000..6c5f5e3a9b1 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp @@ -0,0 +1,56 @@ +/** + * @file fix_state_collision_profile.cpp + * @brief Profile for process that pushes plan instructions to be out of collision + * + * @author Matthew Powelson + * @date August 31. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +FixStateCollisionProfile::FixStateCollisionProfile(Settings mode) + : Profile(FixStateCollisionProfile::getStaticKey()), 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*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FixStateCollisionProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateCollisionProfile) 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 new file mode 100644 index 00000000000..edcf3f7d052 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp @@ -0,0 +1,62 @@ +/** + * @file iterative_spline_parameterization_profile.cpp + * @brief Profile for iterative spline time parameterization + * + * @author Levi Armstrong + * @date August 11. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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_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*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::IterativeSplineParameterizationProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::IterativeSplineParameterizationProfile) diff --git a/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp new file mode 100644 index 00000000000..eeff731d36b --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp @@ -0,0 +1,52 @@ +/** + * @file min_length_profile.cpp + * @brief Profile for task that processing the program so it meets a minimum length. Planners like trajopt + * need at least the user defined number of states in the trajectory to perform velocity, acceleration and jerk + * smoothing. + * + * @author Levi Armstrong + * @date November 2. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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(); } + +template +void MinLengthProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MinLengthProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::MinLengthProfile) diff --git a/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp new file mode 100644 index 00000000000..b70db9798a6 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp @@ -0,0 +1,49 @@ +/** + * @file profile_switch_profile.h + * @brief Profile for task that returns a value based on the profile + * + * @author Matthew Powelson + * @date October 26. 2020 + * @version TODO + * @bug No known bugs + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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(); } + +template +void ProfileSwitchProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ProfileSwitchProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProfileSwitchProfile) 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 new file mode 100644 index 00000000000..d6774911474 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp @@ -0,0 +1,82 @@ +/** + * @file ruckig_trajectory_smoothing_profile.cpp + * @brief Leveraging Ruckig to smooth trajectory + * + * @author Levi Armstrong + * @date July 27, 2022 + * @version TODO + * @bug No known bugs + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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) + , 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() +{ + return std::type_index(typeid(RuckigTrajectorySmoothingCompositeProfile)).hash_code(); +} + +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_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*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +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 new file mode 100644 index 00000000000..04974fc0a2e --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp @@ -0,0 +1,69 @@ +/** + * @file time_optimal_parameterization_profile.cpp + * @brief Profile for TOTG process + * + * @author Levi Armstrong + * @author Matthew Powelson + * @date Jan 22, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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_acceleration_scaling_factor(max_acceleration_scaling_factor) + , max_jerk_scaling_factor(max_jerk_scaling_factor) + , path_tolerance(path_tolerance) + , min_angle_change(min_angle_change) +{ +} + +std::size_t TimeOptimalParameterizationProfile::getStaticKey() +{ + return std::type_index(typeid(TimeOptimalParameterizationProfile)).hash_code(); +} + +template +void TimeOptimalParameterizationProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TimeOptimalParameterizationProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TimeOptimalParameterizationProfile) diff --git a/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp new file mode 100644 index 00000000000..10e536b0baf --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp @@ -0,0 +1,55 @@ +/** + * @file upsample_trajectory_profile.cpp + * + * @author Levi Armstrong + * @date December 15, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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() +{ + return std::type_index(typeid(UpsampleTrajectoryProfile)).hash_code(); +} + +template +void UpsampleTrajectoryProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::UpsampleTrajectoryProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpsampleTrajectoryProfile) diff --git a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp index 02c31f81bd6..55c8d9def93 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -150,7 +150,7 @@ TEST_F(FixStateBoundsTaskUnit, stateInBounds) // NOLINT state_bounds_profile->upper_bounds_reduction = 1e-3; auto profiles = std::make_shared(); - profiles->addProfile(FIX_STATE_BOUNDS_TASK_NAME, DEFAULT_PROFILE_KEY, state_bounds_profile); + profiles->addProfile(FIX_STATE_BOUNDS_TASK_NAME, DEFAULT_PROFILE_KEY, state_bounds_profile); Eigen::VectorXd mid_state = joint_limits.col(0) + (joint_limits.col(1) - joint_limits.col(0)) / 2.; { // All are valid diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index bd046c08306..faf33ef9f25 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -229,8 +229,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask auto profile = std::make_unique(); profile->config.contact_manager_config = tesseract_collision::ContactManagerConfig(1.5); profile->config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - profiles->addProfile( - "TaskComposerContinuousContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); + profiles->addProfile("TaskComposerContinuousContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); @@ -403,8 +402,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe auto profile = std::make_unique(); profile->config.contact_manager_config = tesseract_collision::ContactManagerConfig(1.5); profile->config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - profiles->addProfile( - "TaskComposerDiscreteContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); + profiles->addProfile("TaskComposerDiscreteContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); From 2b16c84ddd97ee41bb03f85c1a7fb1df8d216119 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 6 Dec 2024 13:26:53 -0600 Subject: [PATCH 2/2] Add boost serialization to profiles --- .../tesseract_command_language/profile.h | 2 +- .../profile_dictionary.h | 2 +- .../cartesian_waypoint_poly_unit.hpp | 303 ++++++------ .../test_suite/joint_waypoint_poly_unit.hpp | 409 ++++++++-------- .../test_suite/state_waypoint_poly_unit.hpp | 447 +++++++++--------- .../src/profile_dictionary.cpp | 4 +- tesseract_motion_planners/core/CMakeLists.txt | 5 +- .../descartes/CMakeLists.txt | 3 +- .../profile/descartes_default_plan_profile.h | 5 - .../descartes/profile/descartes_profile.h | 4 - .../descartes_default_plan_profile.cpp | 15 + .../ompl/ompl_planner_configurator.h | 93 ++++ .../ompl/profile/ompl_default_plan_profile.h | 5 - .../ompl/profile/ompl_profile.h | 4 - .../ompl/src/ompl_planner_configurator.cpp | 161 +++++++ .../src/profile/ompl_default_plan_profile.cpp | 15 + .../simple/profile/simple_planner_profile.h | 8 - ...planner_fixed_size_assign_plan_profile.cpp | 2 + ...simple_planner_fixed_size_plan_profile.cpp | 2 + .../simple_planner_lvs_no_ik_plan_profile.cpp | 5 + .../simple_planner_lvs_plan_profile.cpp | 5 + .../trajopt_default_composite_profile.h | 5 - .../profile/trajopt_default_plan_profile.h | 5 - .../profile/trajopt_default_solver_profile.h | 7 - .../trajopt/profile/trajopt_profile.h | 12 - .../trajopt/trajopt_collision_config.h | 15 + .../trajopt/trajopt_waypoint_config.h | 15 + .../trajopt_default_composite_profile.cpp | 17 + .../profile/trajopt_default_plan_profile.cpp | 5 + .../trajopt_default_solver_profile.cpp | 5 + .../trajopt/src/trajopt_collision_config.cpp | 29 ++ .../trajopt/src/trajopt_waypoint_config.cpp | 27 ++ .../trajopt_ifopt_default_solver_profile.h | 4 +- .../profile/trajopt_ifopt_profile.h | 12 - ...rajopt_ifopt_default_composite_profile.cpp | 16 + .../trajopt_ifopt_default_plan_profile.cpp | 5 + .../trajopt_ifopt_default_solver_profile.cpp | 5 + .../planning/CMakeLists.txt | 3 +- .../src/profiles/contact_check_profile.cpp | 2 + .../src/profiles/fix_state_bounds_profile.cpp | 4 + .../profiles/fix_state_collision_profile.cpp | 7 + ...rative_spline_parameterization_profile.cpp | 2 + .../src/profiles/min_length_profile.cpp | 1 + .../src/profiles/profile_switch_profile.cpp | 1 + .../ruckig_trajectory_smoothing_profile.cpp | 3 + .../time_optimal_parameterization_profile.cpp | 5 + .../profiles/upsample_trajectory_profile.cpp | 1 + 47 files changed, 1051 insertions(+), 661 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h index 6d73f9de740..2a97d726d2e 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 d22194faa75..f3e58c4ba5a 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 e599918da6c..73e0be72e7a 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 ca119e4d089..5353ab1545b 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 991a77d5c57..08a14b851a8 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 e788657905b..30f3ea26b5d 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 949263a39a2..1ee03a59fe1 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 097b694dcfb..5eafd22e6a8 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 a52bdc9ebad..1bdde8ee7d4 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 b880b75f8d9..8f90e755ff3 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 f73cc5d87d4..70d073c23bd 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 87cf2a1421d..c4f3dcfe879 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 fbb7e9f4c71..54d3469cf8a 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 f02e779f548..1d5822238a7 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 d23c36466d8..9488d20ce2a 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 08231b7275d..e8a29f36196 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 fe4ff859739..05dd83a33ee 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 3356d09a848..9bf5c2e4751 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 11c83b703e8..b6efdedb9bb 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 be4a21576c4..5efa23ecb44 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 c38a28264f8..fc54a00112f 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 a45debb90fb..1058890e86e 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 7da66147c04..affec26a22c 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 908911a1361..c69e3add39b 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 f9e332bd107..a5323148c90 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 dd239dfc3c0..c26929f7938 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 eb57605eb94..e09acb3ae89 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 14f12308bd4..8ee1838e8d5 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 039cc388b84..63b96df67f6 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 b620a18ce0f..b38cbbdccd1 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 c725c2b08c0..8a12453479a 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 e10e0ae8467..f2db1b9fb65 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 4cb61a3a966..04ee00a461d 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 55fc37eb31c..2aadefeb10d 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 fc837f100cb..0a9ae17a8f1 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 a2d6994a49a..4627f90d3c8 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 fff3fd303e1..e6471c6dddc 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 6fd736e5fba..d42dc32dd0d 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 1358f2f5015..5288c3e0d30 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 82dc182168a..20d830f90a5 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 6c5f5e3a9b1..3ee61e36348 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 edcf3f7d052..15dfe2a2db1 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 eeff731d36b..8036add1450 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 b70db9798a6..f6ce5d1e071 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 d6774911474..12113f0cf6f 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 04974fc0a2e..63b2c838f35 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 10e536b0baf..514400a7636 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