Revised planner profile interfaces

tesseract_command_language/test/move_instruction_unit.cpp

@@ -296,7 +296,7 @@ TEST(TesseractCommandLanguageMoveInstructionPolyUnit, ProfileOverrides)  // NOLI
   Eigen::VectorXd jv = Eigen::VectorXd::Ones(6);
   std::vector<std::string> jn = { "j1", "j2", "j3", "j4", "j5", "j6" };
   StateWaypointPoly swp(StateWaypoint(jn, jv));
-  MoveInstruction instr(swp, MoveInstructionType::START, DEFAULT_PROFILE_KEY, DEFAULT_PROFILE_KEY);
+  MoveInstruction instr(swp, MoveInstructionType::FREESPACE, DEFAULT_PROFILE_KEY, DEFAULT_PROFILE_KEY);
 
   // Create arbitrary profile overrides under arbitrary namespaces
   const std::string ns1 = "ns1";

tesseract_examples/src/car_seat_example.cpp

@@ -283,7 +283,7 @@ bool CarSeatExample::run()
     StateWaypointPoly wp1{ StateWaypoint(joint_group->getJointNames(), pick_pose) };
 
     // Start Joint Position for the program
-    MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE");
+    MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, DEFAULT_PROFILE_KEY);
     start_instruction.setDescription("Start Instruction");
 
     // Plan freespace from start
@@ -369,7 +369,7 @@ bool CarSeatExample::run()
     StateWaypointPoly wp1{ StateWaypoint(joint_group->getJointNames(), pick_pose) };
 
     // Start Joint Position for the program
-    MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE");
+    MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, DEFAULT_PROFILE_KEY);
     start_instruction.setDescription("Start Instruction");
 
     // Plan freespace from start

tesseract_examples/src/glass_upright_example.cpp

@@ -152,7 +152,7 @@ bool GlassUprightExample::run()
   StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) };
   StateWaypointPoly wp1{ StateWaypoint(joint_names, joint_end_pos) };
 
-  MoveInstruction start_instruction(wp0, MoveInstructionType::LINEAR, "UPRIGHT");
+  MoveInstruction start_instruction(wp0, MoveInstructionType::LINEAR, DEFAULT_PROFILE_KEY);
   start_instruction.setDescription("Start Instruction");
 
   // Plan freespace from start

tesseract_motion_planners/core/CMakeLists.txt

@@ -2,7 +2,13 @@ find_package(tesseract_environment REQUIRED)
 find_package(tesseract_command_language REQUIRED)
 
 # Create interface for core
-add_library(${PROJECT_NAME}_core src/core/planner.cpp src/core/utils.cpp src/core/interpolation.cpp)
+add_library(
+  ${PROJECT_NAME}_core
+  src/core/interpolation.cpp
+  src/core/planner.cpp
+  src/core/profiles.cpp
+  src/core/profile_dictionary.cpp
+  src/core/utils.cpp)
 target_link_libraries(
   ${PROJECT_NAME}_core
   PUBLIC tesseract::tesseract_environment

tesseract_motion_planners/core/include/tesseract_motion_planners/core/profile_dictionary.h

@@ -0,0 +1,103 @@
+/**
+ * @file profile_dictionary.h
+ * @brief This is a profile dictionary for storing all profiles
+ *
+ * @author Michael Ripperger
+ * @date January 11, 2022
+ * @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_CORE_PROFILE_DICTIONARY_H
+#define TESSERACT_MOTION_PLANNERS_CORE_PROFILE_DICTIONARY_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <any>
+#include <boost/serialization/serialization.hpp>
+#include <iostream>
+#include <unordered_map>
+#include <memory>
+#include <shared_mutex>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#ifdef SWIG
+%shared_ptr(tesseract_planning::ProfileDictionary)
+#endif  // SWIG
+#include <tesseract_motion_planners/core/profiles.h>
+
+namespace tesseract_planning::tmp
+{
+/**
+ * @brief Stores profiles for motion planning and process planning
+ * @details This class is thread-safe
+ */
+template <typename ProfileT>
+class ProfileDictionary
+{
+public:
+  using Ptr = std::shared_ptr<ProfileDictionary<ProfileT>>;
+  using ConstPtr = std::shared_ptr<const ProfileDictionary<ProfileT>>;
+
+  ProfileDictionary() = default;
+  virtual ~ProfileDictionary() = default;
+
+  ProfileDictionary(const ProfileDictionary& rhs);
+  ProfileDictionary(ProfileDictionary&& rhs) noexcept;
+  ProfileDictionary& operator=(const ProfileDictionary& rhs);
+  ProfileDictionary& operator=(ProfileDictionary&& rhs) noexcept;
+
+  /**
+   * @brief Checks if a profile exists in the provided namespace
+   * @return True if exists, false otherwise
+   */
+  bool hasProfile(const std::string& ns, const std::string& profile) const;
+
+  /**
+   * @brief Gets a profile by name under a given namespace
+   * @throws Runtime exception if the namespace or profile do not exist
+   */
+  typename ProfileT::ConstPtr getProfile(const std::string& ns, const std::string& profile_name) const;
+
+  /**
+   * @brief Gets a profile entry from a given namespace
+   */
+  std::unordered_map<std::string, typename ProfileT::ConstPtr> getProfileEntry(const std::string& ns) const;
+
+  /**
+   * @brief Adds a profile with the input name under the given namespace
+   */
+  void addProfile(const std::string& ns, const std::string& profile_name, typename ProfileT::ConstPtr profile);
+
+  /**
+   * @brief Removes a profile entry from a given namespace
+   */
+  void removeProfile(const std::string& ns, const std::string& profile);
+
+protected:
+  std::unordered_map<std::string, std::unordered_map<std::string, typename ProfileT::ConstPtr>> profiles_;
+  mutable std::shared_mutex mutex_;
+};
+
+using WaypointProfileDictionary = ProfileDictionary<WaypointProfile>;
+using CompositeProfileDictionary = ProfileDictionary<CompositeProfile>;
+using PlannerProfileDictionary = ProfileDictionary<PlannerProfile>;
+
+}  // namespace tesseract_planning::tmp
+
+#endif  // TESSERACT_MOTION_PLANNERS_CORE_PROFILE_DICTIONARY_H

tesseract_motion_planners/core/include/tesseract_motion_planners/core/profiles.h

@@ -0,0 +1,131 @@
+/**
+ * @file profile_dictionary.h
+ * @brief This is a profile dictionary for storing all profiles
+ *
+ * @author Levi Armstrong
+ * @date December 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.
+ */
+#ifndef TESSERACT_MOTION_PLANNERS_CORE_PROFILES_H
+#define TESSERACT_MOTION_PLANNERS_CORE_PROFILES_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <any>
+#include <iostream>
+#include <typeindex>
+#include <unordered_map>
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_environment/environment.h>
+#include <tesseract_command_language/move_instruction.h>
+#include <tesseract_command_language/composite_instruction.h>
+
+namespace tesseract_planning
+{
+/**
+ * @brief Struct to produce a planner-specific planning profile to apply to a single waypoint.
+ * @details Examples of waypoint profiles might include costs/constraints for a waypoint or a waypoint sampler
+ */
+class WaypointProfile
+{
+public:
+  using Ptr = std::shared_ptr<WaypointProfile>;
+  using ConstPtr = std::shared_ptr<const WaypointProfile>;
+
+  WaypointProfile() = default;
+  WaypointProfile(const WaypointProfile&) = delete;
+  WaypointProfile& operator=(const WaypointProfile&) = delete;
+  WaypointProfile(WaypointProfile&&) = delete;
+  WaypointProfile&& operator=(WaypointProfile&&) = delete;
+
+  virtual ~WaypointProfile() = default;
+
+  virtual std::any create(const MoveInstruction& instruction,
+                          tesseract_environment::Environment::ConstPtr env) const = 0;
+
+protected:
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
+};
+
+/**
+ * @brief Struct to produce a planner-specific planning profile to apply to a collection of waypoints defined in a
+ * composite instruction.
+ * @details Examples of composite profiles include costs/constraints that apply collectively to a group of waypoints
+ */
+class CompositeProfile
+{
+public:
+  using Ptr = std::shared_ptr<CompositeProfile>;
+  using ConstPtr = std::shared_ptr<const CompositeProfile>;
+
+  CompositeProfile() = default;
+  CompositeProfile(const CompositeProfile&) = delete;
+  CompositeProfile& operator=(const CompositeProfile&) = delete;
+  CompositeProfile(CompositeProfile&&) = delete;
+  CompositeProfile&& operator=(CompositeProfile&&) = delete;
+
+  virtual ~CompositeProfile() = default;
+  virtual std::any create(const CompositeInstruction& instruction,
+                          tesseract_environment::Environment::ConstPtr env) const = 0;
+
+protected:
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
+};
+
+/**
+ * @brief Struct to produce configuration parameters for the motion planner
+ */
+struct PlannerProfile
+{
+public:
+  using Ptr = std::shared_ptr<PlannerProfile>;
+  using ConstPtr = std::shared_ptr<const PlannerProfile>;
+
+  PlannerProfile() = default;
+  PlannerProfile(const PlannerProfile&) = delete;
+  PlannerProfile& operator=(const PlannerProfile&) = delete;
+  PlannerProfile(PlannerProfile&&) = delete;
+  PlannerProfile&& operator=(PlannerProfile&&) = delete;
+
+  virtual ~PlannerProfile() = default;
+
+  virtual std::any create() const = 0;
+
+protected:
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
+};
+
+}  // namespace tesseract_planning
+
+BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::WaypointProfile);
+BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::CompositeProfile);
+BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::PlannerProfile);
+
+#endif  // TESSERACT_MOTION_PLANNERS_CORE_PROFILES_H

tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h

@@ -32,6 +32,8 @@
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_command_language/profile_dictionary.h>
 
+#include <tesseract_motion_planners/core/profile_dictionary.h>
+
 namespace tesseract_planning
 {
 struct PlannerRequest
@@ -52,6 +54,10 @@ struct PlannerRequest
   /** @brief The profile dictionary */
   ProfileDictionary::ConstPtr profiles{ std::make_shared<ProfileDictionary>() };
 
+  tmp::WaypointProfileDictionary::ConstPtr waypoint_profiles{ std::make_shared<tmp::WaypointProfileDictionary>() };
+  tmp::CompositeProfileDictionary::ConstPtr composite_profiles{ std::make_shared<tmp::CompositeProfileDictionary>() };
+  tmp::PlannerProfileDictionary::ConstPtr planner_profiles{ std::make_shared<tmp::PlannerProfileDictionary>() };
+
   /**
    * @brief The program instruction
    * This must contain a minimum of two move instruction the first move instruction is the start state