From 29ba1edc770cbf30a83bbbb8effdc77e5112bb56 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 7 Dec 2024 15:35:31 -0600 Subject: [PATCH] Add simple ompl profile interface --- tesseract_motion_planners/ompl/CMakeLists.txt | 4 +- .../ompl/deserialize.h | 55 --- .../ompl/ompl_motion_planner.h | 8 - .../ompl/profile/ompl_default_plan_profile.h | 43 +- .../ompl/profile/ompl_profile.h | 50 +- .../ompl/serialize.h | 51 --- .../ompl/src/deserialize.cpp | 127 ------ .../ompl/src/ompl_motion_planner.cpp | 153 +------ .../src/profile/ompl_default_plan_profile.cpp | 430 +++++------------- .../ompl/src/serialize.cpp | 83 ---- .../ompl/test/ompl_planner_tests.cpp | 33 -- 11 files changed, 162 insertions(+), 875 deletions(-) delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h delete mode 100644 tesseract_motion_planners/ompl/src/deserialize.cpp delete mode 100644 tesseract_motion_planners/ompl/src/serialize.cpp diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index ae481465b3..ee1c8f32d0 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -14,9 +14,7 @@ set(OMPL_SRC src/profile/ompl_default_plan_profile.cpp src/utils.cpp src/state_collision_validator.cpp - src/compound_state_validator.cpp - src/serialize.cpp - src/deserialize.cpp) + src/compound_state_validator.cpp) # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif() diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h deleted file mode 100644 index cc6019a933..0000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h +++ /dev/null @@ -1,55 +0,0 @@ -/** - * @file deserialize.h - * @brief Provide methods for deserialize ompl plans to xml - * - * @author Tyler Marr - * @date August 24, 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. - */ -#ifndef TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -class OMPLDefaultPlanProfile; - -OMPLDefaultPlanProfile omplPlanParser(const tinyxml2::XMLElement& xml_element); - -OMPLDefaultPlanProfile omplPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -OMPLDefaultPlanProfile omplPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -OMPLDefaultPlanProfile omplPlanFromXMLFile(const std::string& file_path); - -OMPLDefaultPlanProfile omplPlanFromXMLString(const std::string& xml_string); - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index b8f57fb04a..6a9ce33902 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -94,14 +94,6 @@ class OMPLMotionPlanner : public MotionPlanner protected: /** @brief OMPL Parallel planner */ std::shared_ptr parallel_plan_; - - OMPLProblemConfig createSubProblem(const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& composite_mi, - const std::shared_ptr& manip, - const MoveInstructionPoly& start_instruction, - const MoveInstructionPoly& end_instruction, - int n_output_states, - int index) const; }; } // namespace tesseract_planning 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 54d3469cf8..3acd36e75f 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,9 +62,7 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile using ConstPtr = std::shared_ptr; OMPLDefaultPlanProfile(); - OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); - /** @brief The OMPL state space to use when planning */ OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE }; /** @brief Max planning time allowed in seconds */ @@ -104,53 +102,42 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile /** @brief The collision check configuration */ tesseract_collision::CollisionCheckConfig collision_check_config; + std::unique_ptr create(const PlannerRequest& request, + const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + int n_output_states) const override; + +protected: /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */ - StateSamplerAllocator state_sampler_allocator; + StateSamplerAllocator state_sampler_allocator_; /** @brief Set the optimization objective function allocator. Default is to minimize path length */ - OptimizationObjectiveAllocator optimization_objective_allocator; + OptimizationObjectiveAllocator optimization_objective_allocator_; /** @brief The ompl state validity checker. If nullptr and collision checking enabled it uses * StateCollisionValidator */ - StateValidityCheckerAllocator svc_allocator; + StateValidityCheckerAllocator svc_allocator_; /** @brief The ompl motion validator. If nullptr and continuous collision checking enabled it used * ContinuousMotionValidator */ - MotionValidatorAllocator mv_allocator; + MotionValidatorAllocator mv_allocator_; - void setup(OMPLProblem& prob) const override; + void setup(OMPLProblem& prob) const; void applyGoalStates(OMPLProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + const tesseract_common::ManipulatorInfo& manip_info) const; - void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + void applyGoalStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) const; void applyStartStates(OMPLProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + const tesseract_common::ManipulatorInfo& manip_info) const; - void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; + void applyStartStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) const; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - -protected: ompl::base::StateValidityCheckerPtr processStateValidator(OMPLProblem& prob) const; void processMotionValidator(OMPLProblem& prob, const ompl::base::StateValidityCheckerPtr& svc_without_collision) const; diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h index 1d5822238a..f05a49eef8 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 @@ -34,7 +34,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include #include +#include + #include namespace tinyxml2 @@ -46,6 +50,7 @@ class XMLDocument; namespace tesseract_planning { struct OMPLProblem; + class OMPLPlanProfile : public Profile { public: @@ -60,37 +65,18 @@ class OMPLPlanProfile : public Profile */ static std::size_t getStaticKey(); - virtual void setup(OMPLProblem& prob) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + /** + * @brief Create OMPL Planning Problem + * @param request The planning request + * @param start_instruction The start instruction + * @param end_instruction The goal instruction + * @param n_output_states The number of interpolated output states + * @return A OMPL Problem + */ + virtual std::unique_ptr create(const PlannerRequest& request, + const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + int n_output_states) const = 0; protected: friend class boost::serialization::access; @@ -98,8 +84,6 @@ class OMPLPlanProfile : public Profile 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) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h deleted file mode 100644 index b3b633583a..0000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h +++ /dev/null @@ -1,51 +0,0 @@ -/** - * @file serialize.h - * @brief Provide methods for serializing ompl plans to xml - * - * @author Tyler Marr - * @date August 24, 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. - */ -#ifndef TESSERACT_MOTION_PLANNERS_OMPL_SERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_SERIALIZE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -class OMPLPlanProfile; - -std::shared_ptr toXMLDocument(const OMPLPlanProfile& plan_profile); - -bool toXMLFile(const OMPLPlanProfile& plan_profile, const std::string& file_path); - -std::string toXMLString(const OMPLPlanProfile& plan_profile); - -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_OMPL_SERIALIZE_H diff --git a/tesseract_motion_planners/ompl/src/deserialize.cpp b/tesseract_motion_planners/ompl/src/deserialize.cpp deleted file mode 100644 index 744c63a9af..0000000000 --- a/tesseract_motion_planners/ompl/src/deserialize.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/** - * @file deserialize.cpp - * @brief Provide methods for deserialize instructions to xml and deserialization - * - * @author Tyler Marr - * @date August 21, 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 -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -namespace tesseract_planning -{ -OMPLDefaultPlanProfile omplPlanParser(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* ompl_plan_element = xml_element.FirstChildElement("OMPLPlanProfile"); - return OMPLDefaultPlanProfile{ *ompl_plan_element }; -} - -OMPLDefaultPlanProfile omplPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml) -{ - std::array version{ 0, 0, 0 }; - std::string version_string; - int status = tesseract_common::QueryStringAttribute(profile_xml, "version", version_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - if (status != tinyxml2::XML_NO_ATTRIBUTE) - { - std::vector tokens; - boost::split(tokens, version_string, boost::is_any_of("."), boost::token_compress_on); - if (tokens.size() < 2 || tokens.size() > 3 || !tesseract_common::isNumeric(tokens)) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - tesseract_common::toNumeric(tokens[0], version[0]); - tesseract_common::toNumeric(tokens[1], version[1]); - if (tokens.size() == 3) - tesseract_common::toNumeric(tokens[2], version[2]); - else - version[2] = 0; - } - else - { - CONSOLE_BRIDGE_logWarn("No version number was provided so latest parser will be used."); - } - - const tinyxml2::XMLElement* planner_xml = profile_xml->FirstChildElement("Planner"); - if (planner_xml == nullptr) - throw std::runtime_error("fromXML: Could not find the 'Planner' element in the xml file."); - - int type{ 0 }; - status = planner_xml->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Failed to parse instruction type attribute."); - - return omplPlanParser(*planner_xml); -} - -OMPLDefaultPlanProfile omplPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc) -{ - const tinyxml2::XMLElement* planner_xml = xml_doc.FirstChildElement("Profile"); - if (planner_xml == nullptr) - throw std::runtime_error("Could not find the 'Profile' element in the xml file"); - - return omplPlanFromXMLElement(planner_xml); -} - -OMPLDefaultPlanProfile omplPlanFromXMLString(const std::string& xml_string) -{ - tinyxml2::XMLDocument xml_doc; - tinyxml2::XMLError status = xml_doc.Parse(xml_string.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - throw std::runtime_error("Could not parse the Planner Profile XML File."); - - return omplPlanFromXMLDocument(xml_doc); -} - -OMPLDefaultPlanProfile omplPlanFromXMLFile(const std::string& file_path) -{ - // get the entire file - std::string xml_string; - std::fstream xml_file(file_path.c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - return omplPlanFromXMLString(xml_string); - } - - throw std::runtime_error("Could not open file " + file_path + "for parsing."); -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 2532d3fdd4..6516b9a463 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -320,105 +320,6 @@ void OMPLMotionPlanner::clear() { parallel_plan_ = nullptr; } std::unique_ptr OMPLMotionPlanner::clone() const { return std::make_unique(name_); } -OMPLProblemConfig -OMPLMotionPlanner::createSubProblem(const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& composite_mi, - const std::shared_ptr& manip, - const MoveInstructionPoly& start_instruction, - const MoveInstructionPoly& end_instruction, - int n_output_states, - int index) const -{ - std::vector joint_names = manip->getJointNames(); - std::vector active_link_names = manip->getActiveLinkNames(); - - // Get Plan Profile - auto cur_plan_profile = getProfile( - name_, end_instruction.getProfile(name_), *request.profiles, std::make_shared()); - - if (!cur_plan_profile) - throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); - - /** @todo Should check that the joint names match the order of the manipulator */ - OMPLProblemConfig config; - config.start_uuid = start_instruction.getUUID(); - config.end_uuid = end_instruction.getUUID(); - config.problem = std::make_shared(); - config.problem->env = request.env; - config.problem->env_state = request.env_state; - config.problem->manip = manip; - config.problem->contact_checker = request.env->getDiscreteContactManager(); - config.problem->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); - config.problem->contact_checker->setActiveCollisionObjects(active_link_names); - - cur_plan_profile->setup(*config.problem); - config.problem->n_output_states = n_output_states; - - if (end_instruction.getWaypoint().isJointWaypoint() || end_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, end_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(end_instruction.getWaypoint()); - cur_plan_profile->applyGoalStates( - *config.problem, cur_position, end_instruction, composite_mi, active_link_names, index); - - if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); - const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); - cur_plan_profile->applyStartStates( - *config.problem, prev_position, start_instruction, composite_mi, active_link_names, index); - } - else if (start_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& prev_wp = start_instruction.getWaypoint().as(); - cur_plan_profile->applyStartStates( - *config.problem, prev_wp.getTransform(), start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); - } - - return config; - } - - if (end_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& cur_wp = end_instruction.getWaypoint().as(); - cur_plan_profile->applyGoalStates( - *config.problem, cur_wp.getTransform(), end_instruction, composite_mi, active_link_names, index); - - if (index == 0) - { - if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); - const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); - cur_plan_profile->applyStartStates( - *config.problem, prev_position, start_instruction, composite_mi, active_link_names, index); - } - else if (start_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& prev_wp = start_instruction.getWaypoint().as(); - cur_plan_profile->applyStartStates( - *config.problem, prev_wp.getTransform(), start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); - } - } - else - { - /** @todo Update. Extract the solution for the previous plan and set as the start */ - assert(false); - } - - return config; - } - - throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); -} std::vector OMPLMotionPlanner::createProblems(const PlannerRequest& request) const { std::vector problems; @@ -426,49 +327,10 @@ std::vector OMPLMotionPlanner::createProblems(const PlannerRe // Assume all the plan instructions have the same manipulator as the composite assert(!request.instructions.getManipulatorInfo().empty()); - const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); - - tesseract_kinematics::JointGroup::Ptr manip; - if (composite_mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); - - try - { - tesseract_kinematics::KinematicGroup::Ptr kin_group; - std::string error_msg; - if (composite_mi.manipulator_ik_solver.empty()) - { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; - } - else - { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "' with solver '" + - composite_mi.manipulator_ik_solver + "'"; - } - - if (kin_group == nullptr) - { - CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); - throw std::runtime_error(error_msg); - } - - manip = kin_group; - } - catch (...) - { - manip = request.env->getJointGroup(composite_mi.manipulator); - } - - if (!manip) - throw std::runtime_error("Failed to get joint/kinematic group: " + composite_mi.manipulator); - // Flatten the input for planning auto move_instructions = request.instructions.flatten(&moveFilter); // Transform plan instructions into ompl problem - int index = 0; int num_output_states = 1; MoveInstructionPoly start_instruction = move_instructions.front().get().as(); @@ -482,8 +344,19 @@ std::vector OMPLMotionPlanner::createProblems(const PlannerRe if (waypoint.isCartesianWaypoint() || waypoint.isStateWaypoint() || (waypoint.isJointWaypoint() && waypoint.as().isConstrained())) { - problems.push_back(createSubProblem( - request, composite_mi, manip, start_instruction, move_instruction, num_output_states, index++)); + // Get Plan Profile + auto cur_plan_profile = getProfile( + name_, move_instruction.getProfile(name_), *request.profiles, std::make_shared()); + + if (!cur_plan_profile) + throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); + + OMPLProblemConfig config; + config.start_uuid = start_instruction.getUUID(); + config.end_uuid = move_instruction.getUUID(); + config.problem = cur_plan_profile->create(request, start_instruction, move_instruction, num_output_states); + + problems.push_back(config); start_instruction = move_instruction; num_output_states = 1; } 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 a3f164a3b0..113bcc75b4 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 @@ -40,7 +40,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include #include #include @@ -64,240 +66,122 @@ OMPLDefaultPlanProfile::OMPLDefaultPlanProfile() { } -OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) - : planners({ std::make_shared(), std::make_shared() }) +std::unique_ptr OMPLDefaultPlanProfile::create(const PlannerRequest& request, + const MoveInstructionPoly& start_instruction, + const MoveInstructionPoly& end_instruction, + int n_output_states) const { - const tinyxml2::XMLElement* state_space_element = xml_element.FirstChildElement("StateSpace"); - const tinyxml2::XMLElement* planning_time_element = xml_element.FirstChildElement("PlanningTime"); - const tinyxml2::XMLElement* max_solutions_element = xml_element.FirstChildElement("MaxSolutions"); - const tinyxml2::XMLElement* simplify_element = xml_element.FirstChildElement("Simplify"); - const tinyxml2::XMLElement* optimize_element = xml_element.FirstChildElement("Optimize"); - const tinyxml2::XMLElement* planners_element = xml_element.FirstChildElement("Planners"); - // const tinyxml2::XMLElement* collision_check_element = xml_element.FirstChildElement("CollisionCheck"); - // const tinyxml2::XMLElement* collision_continuous_element = xml_element.FirstChildElement("CollisionContinuous"); - // const tinyxml2::XMLElement* collision_safety_margin_element = - // xml_element.FirstChildElement("CollisionSafetyMargin"); - // const tinyxml2::XMLElement* longest_valid_segment_fraction_element = - // xml_element.FirstChildElement("LongestValidSegme" - // "ntFraction"); - // const tinyxml2::XMLElement* longest_valid_segment_length_element = - // xml_element.FirstChildElement("LongestValidSegment" - // "Length"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (state_space_element != nullptr) - { - auto type = static_cast(OMPLProblemStateSpace::REAL_STATE_SPACE); - status = state_space_element->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing StateSpace type attribute."); + const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); - state_space = static_cast(type); - } + tesseract_kinematics::JointGroup::Ptr manip; + if (composite_mi.manipulator.empty()) + throw std::runtime_error("OMPL, manipulator is empty!"); - if (planning_time_element != nullptr) + try { - std::string planning_time_string; - status = tesseract_common::QueryStringText(planning_time_element, planning_time_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing PlanningTime string"); + tesseract_kinematics::KinematicGroup::Ptr kin_group; + std::string error_msg; + if (composite_mi.manipulator_ik_solver.empty()) + { + kin_group = request.env->getKinematicGroup(composite_mi.manipulator); + error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; + } + else + { + kin_group = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); + error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "' with solver '" + + composite_mi.manipulator_ik_solver + "'"; + } - if (!tesseract_common::isNumeric(planning_time_string)) - throw std::runtime_error("OMPLPlanProfile: PlanningTime is not a numeric values."); + if (kin_group == nullptr) + { + CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); + throw std::runtime_error(error_msg); + } - tesseract_common::toNumeric(planning_time_string, planning_time); + manip = kin_group; } - - if (max_solutions_element != nullptr) + catch (...) { - std::string max_solutions_string; - status = tesseract_common::QueryStringText(max_solutions_element, max_solutions_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing MaxSolutions string"); + manip = request.env->getJointGroup(composite_mi.manipulator); + } - if (!tesseract_common::isNumeric(max_solutions_string)) - throw std::runtime_error("OMPLPlanProfile: MaxSolutions is not a numeric values."); + if (!manip) + throw std::runtime_error("Failed to get joint/kinematic group: " + composite_mi.manipulator); - tesseract_common::toNumeric(max_solutions_string, max_solutions); - } + std::vector joint_names = manip->getJointNames(); + std::vector active_link_names = manip->getActiveLinkNames(); - if (simplify_element != nullptr) - { - status = simplify_element->QueryBoolText(&simplify); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Simplify string"); - } + /** @todo Should check that the joint names match the order of the manipulator */ + auto problem = std::make_unique(); + problem->env = request.env; + problem->env_state = request.env_state; + problem->manip = manip; + problem->contact_checker = request.env->getDiscreteContactManager(); + problem->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); + problem->contact_checker->setActiveCollisionObjects(active_link_names); + + setup(*problem); + problem->n_output_states = n_output_states; - if (optimize_element != nullptr) + if (end_instruction.getWaypoint().isJointWaypoint() || end_instruction.getWaypoint().isStateWaypoint()) { - status = optimize_element->QueryBoolText(&optimize); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Optimize string"); + assert(checkJointPositionFormat(joint_names, end_instruction.getWaypoint())); + const Eigen::VectorXd& cur_position = getJointPosition(end_instruction.getWaypoint()); + applyGoalStates(*problem, cur_position); + + if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) + { + assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); + const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); + applyStartStates(*problem, prev_position); + } + else if (start_instruction.getWaypoint().isCartesianWaypoint()) + { + const auto& prev_wp = start_instruction.getWaypoint().as(); + applyStartStates(*problem, prev_wp.getTransform(), start_instruction, composite_mi); + } + else + { + throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); + } + + return problem; } - if (planners_element != nullptr) + if (end_instruction.getWaypoint().isCartesianWaypoint()) { - planners.clear(); - for (const tinyxml2::XMLElement* e = planners_element->FirstChildElement("Planner"); e != nullptr; - e = e->NextSiblingElement("Planner")) - { - int type{ 0 }; - status = e->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Planner type attribute."); + const auto& cur_wp = end_instruction.getWaypoint().as(); + applyGoalStates(*problem, cur_wp.getTransform(), end_instruction, composite_mi); - switch (type) + if (request.instructions.getFirstMoveInstruction()->getUUID() == start_instruction.getUUID()) + { + if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) { - case static_cast(OMPLPlannerType::SBL): - { - SBLConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::EST): - { - ESTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::LBKPIECE1): - { - LBKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::BKPIECE1): - { - BKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::KPIECE1): - { - KPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::BiTRRT): - { - BiTRRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRT): - { - RRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRTConnect): - { - RRTConnectConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRTstar): - { - RRTstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::TRRT): - { - TRRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::PRM): - { - PRMConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::PRMstar): - { - PRMstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::LazyPRMstar): - { - LazyPRMstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::SPARS): - { - SPARSConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - default: - { - throw std::runtime_error("Unsupported OMPL Planner type"); - } + assert(checkJointPositionFormat(joint_names, start_instruction.getWaypoint())); + const Eigen::VectorXd& prev_position = getJointPosition(start_instruction.getWaypoint()); + applyStartStates(*problem, prev_position); + } + else if (start_instruction.getWaypoint().isCartesianWaypoint()) + { + const auto& prev_wp = start_instruction.getWaypoint().as(); + applyStartStates(*problem, prev_wp.getTransform(), start_instruction, composite_mi); + } + else + { + throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); } } + else + { + /** @todo Update. Extract the solution for the previous plan and set as the start */ + assert(false); + } + + return problem; } - /// @todo Update XML - // if (collision_check_element) - // { - // status = collision_check_element->QueryBoolText(&collision_check); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionCheck string"); - // } - - // if (collision_continuous_element) - // { - // status = collision_continuous_element->QueryBoolText(&collision_continuous); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionContinuous string"); - // } - - /// @todo Update XML - // if (collision_safety_margin_element) - // { - // std::string collision_safety_margin_string; - // status = tesseract_common::QueryStringText(collision_safety_margin_element, collision_safety_margin_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionSafetyMargin string"); - - // if (!tesseract_common::isNumeric(collision_safety_margin_string)) - // throw std::runtime_error("OMPLPlanProfile: CollisionSafetyMargin is not a numeric values."); - - // tesseract_common::toNumeric(collision_safety_margin_string, collision_safety_margin); - // } - - // if (longest_valid_segment_fraction_element) - // { - // std::string longest_valid_segment_fraction_string; - // status = tesseract_common::QueryStringText(longest_valid_segment_fraction_element, - // longest_valid_segment_fraction_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentFraction string"); - - // if (!tesseract_common::isNumeric(longest_valid_segment_fraction_string)) - // throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentFraction is not a numeric values."); - - // tesseract_common::toNumeric(longest_valid_segment_fraction_string, longest_valid_segment_fraction); - // } - - // if (longest_valid_segment_length_element) - // { - // std::string longest_valid_segment_length_string; - // status = - // tesseract_common::QueryStringText(longest_valid_segment_length_element, - // longest_valid_segment_length_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentLength string"); - - // if (!tesseract_common::isNumeric(longest_valid_segment_length_string)) - // throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentLength is not a numeric values."); - - // tesseract_common::toNumeric(longest_valid_segment_length_string, longest_valid_segment_length); - // } + throw std::runtime_error("OMPLMotionPlanner: unknown waypoint type"); } void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const @@ -335,10 +219,10 @@ void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const for (unsigned i = 0; i < dof; ++i) rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); - if (state_sampler_allocator) + if (state_sampler_allocator_) { rss->setStateSamplerAllocator( - [=](const ompl::base::StateSpace* space) { return state_sampler_allocator(space, prob); }); + [=](const ompl::base::StateSpace* space) { return state_sampler_allocator_(space, prob); }); } else { @@ -371,9 +255,7 @@ void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& /*active_links*/, - int /*index*/) const + const tesseract_common::ManipulatorInfo& manip_info) const { const auto dof = prob.manip->numJoints(); tesseract_common::KinematicLimits limits = prob.manip->getLimits(); @@ -459,12 +341,7 @@ void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, } } -void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& /*parent_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, - const std::vector& /*active_links*/, - int /*index*/) const +void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) const { const auto dof = prob.manip->numJoints(); tesseract_common::KinematicLimits limits = prob.manip->getLimits(); @@ -506,9 +383,7 @@ void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& /*active_links*/, - int /*index*/) const + const tesseract_common::ManipulatorInfo& manip_info) const { const auto dof = prob.manip->numJoints(); tesseract_common::KinematicLimits limits = prob.manip->getLimits(); @@ -595,12 +470,7 @@ void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, } } -void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const MoveInstructionPoly& /*parent_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, - const std::vector& /*active_links*/, - int /*index*/) const +void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint) const { const auto dof = prob.manip->numJoints(); tesseract_common::KinematicLimits limits = prob.manip->getLimits(); @@ -640,84 +510,13 @@ void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, } } -tinyxml2::XMLElement* OMPLDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const -{ - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(2).c_str()); - - tinyxml2::XMLElement* xml_ompl = doc.NewElement("OMPLPlanProfile"); - - tinyxml2::XMLElement* xml_ompl_planners = doc.NewElement("Planners"); - - for (const auto& planner : planners) - { - tinyxml2::XMLElement* xml_ompl_planner = doc.NewElement("Planner"); - xml_ompl_planner->SetAttribute("type", std::to_string(static_cast(planner->getType())).c_str()); - tinyxml2::XMLElement* xml_planner = planner->toXML(doc); - xml_ompl_planner->InsertEndChild(xml_planner); - xml_ompl_planners->InsertEndChild(xml_ompl_planner); - } - - xml_ompl->InsertEndChild(xml_ompl_planners); - - tinyxml2::XMLElement* xml_state_space = doc.NewElement("StateSpace"); - xml_state_space->SetAttribute("type", std::to_string(static_cast(state_space)).c_str()); - xml_ompl->InsertEndChild(xml_state_space); - - tinyxml2::XMLElement* xml_planning_time = doc.NewElement("PlanningTime"); - xml_planning_time->SetText(planning_time); - xml_ompl->InsertEndChild(xml_planning_time); - - tinyxml2::XMLElement* xml_max_solutions = doc.NewElement("MaxSolutions"); - xml_max_solutions->SetText(max_solutions); - xml_ompl->InsertEndChild(xml_max_solutions); - - tinyxml2::XMLElement* xml_simplify = doc.NewElement("Simplify"); - xml_simplify->SetText(simplify); - xml_ompl->InsertEndChild(xml_simplify); - - tinyxml2::XMLElement* xml_optimize = doc.NewElement("Optimize"); - xml_optimize->SetText(optimize); - xml_ompl->InsertEndChild(xml_optimize); - - /// @todo Update XML - // tinyxml2::XMLElement* xml_collision_check = doc.NewElement("CollisionCheck"); - // xml_collision_check->SetText(collision_check); - // xml_ompl->InsertEndChild(xml_collision_check); - - // tinyxml2::XMLElement* xml_collision_continuous = doc.NewElement("CollisionContinuous"); - // xml_collision_continuous->SetText(collision_continuous); - // xml_ompl->InsertEndChild(xml_collision_continuous); - - // tinyxml2::XMLElement* xml_collision_safety_margin = doc.NewElement("CollisionSafetyMargin"); - // xml_collision_safety_margin->SetText(collision_safety_margin); - // xml_ompl->InsertEndChild(xml_collision_safety_margin); - - // tinyxml2::XMLElement* xml_long_valid_seg_frac = doc.NewElement("LongestValidSegmentFraction"); - // xml_long_valid_seg_frac->SetText(longest_valid_segment_fraction); - // xml_ompl->InsertEndChild(xml_long_valid_seg_frac); - - // tinyxml2::XMLElement* xml_long_valid_seg_len = doc.NewElement("LongestValidSegmentLength"); - // xml_long_valid_seg_len->SetText(longest_valid_segment_length); - // xml_ompl->InsertEndChild(xml_long_valid_seg_len); - - // TODO: Add plugins for state_sampler_allocator, optimization_objective_allocator, svc_allocator, - // mv_allocator - - xml_planner->InsertEndChild(xml_ompl); - - return xml_planner; -} - ompl::base::StateValidityCheckerPtr OMPLDefaultPlanProfile::processStateValidator(OMPLProblem& prob) const { ompl::base::StateValidityCheckerPtr svc_without_collision; auto csvc = std::make_shared(); - if (svc_allocator != nullptr) + if (svc_allocator_ != nullptr) { - svc_without_collision = svc_allocator(prob.simple_setup->getSpaceInformation(), prob); + svc_without_collision = svc_allocator_(prob.simple_setup->getSpaceInformation(), prob); csvc->addStateValidator(svc_without_collision); } @@ -737,9 +536,9 @@ void OMPLDefaultPlanProfile::processMotionValidator( OMPLProblem& prob, const ompl::base::StateValidityCheckerPtr& svc_without_collision) const { - if (mv_allocator != nullptr) + if (mv_allocator_ != nullptr) { - auto mv = mv_allocator(prob.simple_setup->getSpaceInformation(), prob); + auto mv = mv_allocator_(prob.simple_setup->getSpaceInformation(), prob); prob.simple_setup->getSpaceInformation()->setMotionValidator(mv); } else @@ -769,10 +568,10 @@ void OMPLDefaultPlanProfile::processMotionValidator( void OMPLDefaultPlanProfile::processOptimizationObjective(OMPLProblem& prob) const { - if (optimization_objective_allocator) + if (optimization_objective_allocator_) { prob.simple_setup->getProblemDefinition()->setOptimizationObjective( - optimization_objective_allocator(prob.simple_setup->getSpaceInformation(), prob)); + optimization_objective_allocator_(prob.simple_setup->getSpaceInformation(), prob)); } else if (prob.optimize) { @@ -786,17 +585,20 @@ 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); + + // The following should not be serialized because they are provided for customization + // and should be populated as part of the inherited classes default constructor + + // 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/ompl/src/serialize.cpp b/tesseract_motion_planners/ompl/src/serialize.cpp deleted file mode 100644 index bcfe772eaa..0000000000 --- a/tesseract_motion_planners/ompl/src/serialize.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/** - * @file serialize.cpp - * @brief - * - * @author Tyler Marr - * @date August 20, 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 -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -static const std::array VERSION{ { 1, 0, 0 } }; -static const Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - -namespace tesseract_planning -{ -std::shared_ptr toXMLDocument(const OMPLPlanProfile& plan_profile) -{ - auto doc = std::make_shared(); - tinyxml2::XMLElement* xml_root = doc->NewElement("Profile"); - xml_root->SetAttribute("name", "OMPLDefaultProfile"); - xml_root->SetAttribute( - "version", - (std::to_string(VERSION[0]) + "." + std::to_string(VERSION[1]) + "." + std::to_string(VERSION[2])).c_str()); - - tinyxml2::XMLElement* xml_plan_profile = plan_profile.toXML(*doc); - xml_root->InsertEndChild(xml_plan_profile); - doc->InsertFirstChild(xml_root); - - return doc; -} - -bool toXMLFile(const OMPLPlanProfile& plan_profile, const std::string& file_path) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLError status = doc->SaveFile(file_path.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - { - CONSOLE_BRIDGE_logError("Failed to save Plan Profile XML File: %s", file_path.c_str()); - return false; - } - - return true; -} - -std::string toXMLString(const OMPLPlanProfile& plan_profile) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLPrinter printer; - doc->Print(&printer); - return std::string{ printer.CStr() }; -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 45052b5b2b..8bd446f2fe 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -115,39 +115,6 @@ static void addBox(tesseract_environment::Environment& env) env.applyCommand(std::make_shared(link_1, joint_1)); } -TEST(TesseractPlanningOMPLSerializeUnit, SerializeOMPLDefaultPlanToXml) // NOLINT -{ - // Write program to file - OMPLDefaultPlanProfile plan_profile; - plan_profile.simplify = true; - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - plan_profile.planners.push_back(std::make_shared()); - - EXPECT_TRUE(toXMLFile(plan_profile, tesseract_common::getTempPath() + "ompl_default_plan_example_input.xml")); - - // Import file - OMPLDefaultPlanProfile imported_plan_profile = omplPlanFromXMLFile(tesseract_common::getTempPath() + "ompl_default_" - "plan_example_" - "input.xml"); - - // Re-write file and compare changed from default term - EXPECT_TRUE( - toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "ompl_default_plan_example_input2.xml")); - EXPECT_TRUE(plan_profile.simplify == imported_plan_profile.simplify); -} - template class OMPLTestFixture : public ::testing::Test {