tesseract-robotics · Levi-Armstrong · Nov 4, 2022 · Oct 8, 2022 · Oct 8, 2022 · Oct 8, 2022
diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h
@@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/poly/instruction_poly.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 #include <tesseract_command_language/constants.h>
-#include <tesseract_command_language/profile_dictionary.h>
+#include <tesseract_command_language/types.h>
 #include <tesseract_common/manipulator_info.h>
 
 namespace tesseract_planning
@@ -83,10 +83,10 @@ class CompositeInstruction
   void print(const std::string& prefix = "") const;
 
   void setProfile(const std::string& profile);
-  const std::string& getProfile() const;
+  const std::string& getProfile(const std::string& ns = "") const;
 
-  void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
-  ProfileDictionary::ConstPtr getProfileOverrides() const;
+  void setProfileOverrides(ProfileOverrides profile_overrides);
+  ProfileOverrides getProfileOverrides() const;
 
   void setManipulatorInfo(tesseract_common::ManipulatorInfo info);
   const tesseract_common::ManipulatorInfo& getManipulatorInfo() const;
@@ -349,7 +349,7 @@ class CompositeInstruction
   std::string profile_{ DEFAULT_PROFILE_KEY };
 
   /** @brief Dictionary of profiles that will override named profiles for a specific task*/
-  ProfileDictionary::ConstPtr profile_overrides_;
+  ProfileOverrides profile_overrides_;
 
   /** @brief The order of the composite instruction */
   CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED };

diff --git a/tesseract_command_language/include/tesseract_command_language/move_instruction.h b/tesseract_command_language/include/tesseract_command_language/move_instruction.h
@@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 #include <tesseract_command_language/poly/waypoint_poly.h>
 #include <tesseract_command_language/constants.h>
-#include <tesseract_command_language/profile_dictionary.h>
+#include <tesseract_command_language/types.h>
 #include <tesseract_common/manipulator_info.h>
 
 namespace tesseract_planning
@@ -127,16 +127,16 @@ class MoveInstruction
   tesseract_common::ManipulatorInfo& getManipulatorInfo();
 
   void setProfile(const std::string& profile);
-  const std::string& getProfile() const;
+  const std::string& getProfile(const std::string& ns = "") const;
 
   void setPathProfile(const std::string& profile);
-  const std::string& getPathProfile() const;
+  const std::string& getPathProfile(const std::string& ns = "") const;
 
-  void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
-  ProfileDictionary::ConstPtr getProfileOverrides() const;
+  void setProfileOverrides(ProfileOverrides profile_overrides);
+  ProfileOverrides getProfileOverrides() const;
 
-  void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
-  ProfileDictionary::ConstPtr getPathProfileOverrides() const;
+  void setPathProfileOverrides(ProfileOverrides profile_overrides);
+  ProfileOverrides getPathProfileOverrides() const;
 
   const std::string& getDescription() const;
 
@@ -171,10 +171,10 @@ class MoveInstruction
   std::string path_profile_;
 
   /** @brief Dictionary of profiles that will override named profiles for a specific task*/
-  ProfileDictionary::ConstPtr profile_overrides_;
+  ProfileOverrides profile_overrides_;
 
   /** @brief Dictionary of path profiles that will override named profiles for a specific task*/
-  ProfileDictionary::ConstPtr path_profile_overrides_;
+  ProfileOverrides path_profile_overrides_;
 
   /** @brief The assigned waypoint (Cartesian, Joint or State) */
   WaypointPoly waypoint_;

diff --git a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h
@@ -40,7 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/poly/joint_waypoint_poly.h>
 #include <tesseract_command_language/poly/state_waypoint_poly.h>
 #include <tesseract_command_language/poly/waypoint_poly.h>
-#include <tesseract_command_language/profile_dictionary.h>
+#include <tesseract_command_language/types.h>
 #include <tesseract_common/manipulator_info.h>
 #include <tesseract_common/serialization.h>
 #include <tesseract_common/type_erasure.h>
@@ -132,11 +132,11 @@ struct MoveInstructionConcept  // NOLINT
     const std::string& path_profile_const = c.getPathProfile();
     UNUSED(path_profile_const);
 
-    c.setProfileOverrides(nullptr);
+    c.setProfileOverrides(ProfileOverrides{});
     auto profile_overrides = c.getProfileOverrides();
     UNUSED(profile_overrides);
 
-    c.setPathProfileOverrides(nullptr);
+    c.setPathProfileOverrides(ProfileOverrides{});
     auto path_profile_overrides = c.getPathProfileOverrides();
     UNUSED(path_profile_overrides);
 
@@ -184,16 +184,16 @@ struct MoveInstructionInterface : tesseract_common::TypeErasureInterface
   virtual tesseract_common::ManipulatorInfo& getManipulatorInfo() = 0;
 
   virtual void setProfile(const std::string& profile) = 0;
-  virtual const std::string& getProfile() const = 0;
+  virtual const std::string& getProfile(const std::string& ns = "") const = 0;
 
   virtual void setPathProfile(const std::string& profile) = 0;
-  virtual const std::string& getPathProfile() const = 0;
+  virtual const std::string& getPathProfile(const std::string& ns = "") const = 0;
 
-  virtual void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0;
-  virtual ProfileDictionary::ConstPtr getProfileOverrides() const = 0;
+  virtual void setProfileOverrides(ProfileOverrides profile_overrides) = 0;
+  virtual ProfileOverrides getProfileOverrides() const = 0;
 
-  virtual void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0;
-  virtual ProfileDictionary::ConstPtr getPathProfileOverrides() const = 0;
+  virtual void setPathProfileOverrides(ProfileOverrides profile_overrides) = 0;
+  virtual ProfileOverrides getPathProfileOverrides() const = 0;
 
   virtual void setMoveType(MoveInstructionType move_type) = 0;
   virtual MoveInstructionType getMoveType() const = 0;
@@ -247,22 +247,22 @@ struct MoveInstructionInstance : tesseract_common::TypeErasureInstance<T, MoveIn
   tesseract_common::ManipulatorInfo& getManipulatorInfo() final { return this->get().getManipulatorInfo(); }
 
   void setProfile(const std::string& profile) final { this->get().setProfile(profile); }
-  const std::string& getProfile() const final { return this->get().getProfile(); }
+  const std::string& getProfile(const std::string& ns = "") const final { return this->get().getProfile(ns); }
 
   void setPathProfile(const std::string& profile) final { this->get().setPathProfile(profile); }
-  const std::string& getPathProfile() const final { return this->get().getPathProfile(); }
+  const std::string& getPathProfile(const std::string& ns = "") const final { return this->get().getPathProfile(ns); }
 
-  void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final
+  void setProfileOverrides(ProfileOverrides profile_overrides) final
   {
     this->get().setProfileOverrides(profile_overrides);
   }
-  ProfileDictionary::ConstPtr getProfileOverrides() const final { return this->get().getProfileOverrides(); }
+  ProfileOverrides getProfileOverrides() const final { return this->get().getProfileOverrides(); }
 
-  void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final
+  void setPathProfileOverrides(ProfileOverrides profile_overrides) final
   {
     this->get().setPathProfileOverrides(profile_overrides);
   }
-  ProfileDictionary::ConstPtr getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); }
+  ProfileOverrides getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); }
 
   void setMoveType(MoveInstructionType move_type) final { this->get().setMoveType(move_type); }
   MoveInstructionType getMoveType() const final { return this->get().getMoveType(); }
@@ -312,16 +312,16 @@ struct MoveInstructionPoly : MoveInstructionPolyBase
   tesseract_common::ManipulatorInfo& getManipulatorInfo();
 
   void setProfile(const std::string& profile);
-  const std::string& getProfile() const;
+  const std::string& getProfile(const std::string& ns = "") const;
 
   void setPathProfile(const std::string& profile);
-  const std::string& getPathProfile() const;
+  const std::string& getPathProfile(const std::string& ns = "") const;
 
-  void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
-  ProfileDictionary::ConstPtr getProfileOverrides() const;
+  void setProfileOverrides(ProfileOverrides profile_overrides);
+  ProfileOverrides getProfileOverrides() const;
 
-  void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides);
-  ProfileDictionary::ConstPtr getPathProfileOverrides() const;
+  void setPathProfileOverrides(ProfileOverrides profile_overrides);
+  ProfileOverrides getPathProfileOverrides() const;
 
   void setMoveType(MoveInstructionType move_type);
   MoveInstructionType getMoveType() const;

diff --git a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h
@@ -35,19 +35,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <memory>
 #include <mutex>
 #include <shared_mutex>
+#include <sstream>
+#include <boost/core/demangle.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-/**
- * This used to store specific profile mapping with the request
- *
- * For example say you have a profile named Raster in your command language. Say you have multiple Raster profiles
- * for descartes {Raster, Raster1, Raster2}. This allows you to remap the meaning of Raster in the command language to
- * say Raster2 for the specific planner Descartes by Map<Descartes, Map<Raster, Raster1>>.
- */
-using ProfileRemapping = std::unordered_map<std::string, std::unordered_map<std::string, std::string>>;
-
 /**
  * @brief This class is used to store profiles for motion planning and process planning
  * @details This is a thread safe class
@@ -156,31 +149,6 @@ class ProfileDictionary
     }
   }
 
-  /**
-   * @brief Check if a profile exists
-   * @details If profile entry does not exist it also returns false
-   * @return True if profile exists, otherwise false
-   */
-  template <typename ProfileType>
-  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<const std::unordered_map<std::string, std::shared_ptr<const ProfileType>>&>(it2->second);
-      auto it3 = profile_map.find(profile_name);
-      if (it3 != profile_map.end())
-        return true;
-    }
-    return false;
-  }
-
   /**
    * @brief Get a profile by name
    * @details Check if the profile exist before calling this function, if missing an exception is thrown
@@ -191,11 +159,50 @@ class ProfileDictionary
   std::shared_ptr<const ProfileType> 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 std::unordered_map<std::type_index, std::any>* it = nullptr;
+    try
+    {
+      it = &(profiles_.at(ns));
+    }
+    catch (const std::exception&)
+    {
+      std::stringstream ss;
+      ss << "Failed to find entries for namespace '" << ns << "'. Available namespaces are: ";
+      for (auto it = profiles_.begin(); it != profiles_.end(); ++it)
+      {
+        ss << "'" << it->first << "', ";
+      }
+      std::throw_with_nested(std::runtime_error(ss.str()));
+    }
+
+    const std::any* it2 = nullptr;
+    try
+    {
+      it2 = &(it->at(std::type_index(typeid(ProfileType))));
+    }
+    catch (const std::exception&)
+    {
+      std::stringstream ss;
+      ss << "No entries for profile base class type '" << boost::core::demangle(typeid(ProfileType).name()) << "'";
+      std::throw_with_nested(std::runtime_error(ss.str()));
+    }
+
     const auto& profile_map =
-        std::any_cast<const std::unordered_map<std::string, std::shared_ptr<const ProfileType>>&>(it2);
-    return profile_map.at(profile_name);
+        std::any_cast<const std::unordered_map<std::string, std::shared_ptr<const ProfileType>>&>(*it2);
+    try
+    {
+      return profile_map.at(profile_name);
+    }
+    catch (const std::exception&)
+    {
+      std::stringstream ss;
+      ss << "No entries for profile '" << profile_name << "' in namespace '" << ns << "'. Available profiles are: ";
+      for (auto it = profile_map.begin(); it != profile_map.end(); ++it)
+        ss << "'" << it->first << "', ";
+
+      std::throw_with_nested(std::runtime_error(ss.str()));
+    }
   }
 
   /**

diff --git a/tesseract_command_language/include/tesseract_command_language/types.h b/tesseract_command_language/include/tesseract_command_language/types.h
@@ -31,6 +31,16 @@
 namespace tesseract_planning
 {
 using ManipulatorInfo = tesseract_common::ManipulatorInfo;
+
+/**
+ * @brief Map that associates for override profile names (values) with specified task namespaces (keys)
+ * @details For example, a profile "default" might have the following override profiles names for various specific task
+ * namespaces
+ *   - ["ompl", "custom_profile_1"]
+ *   - ["time_parameterization", "custom_profile_2"]
+ */
+using ProfileOverrides = std::unordered_map<std::string, std::string>;
+
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_COMMAND_LANGUAGE_TYPES_H
diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp
@@ -69,13 +69,19 @@ void CompositeInstruction::setProfile(const std::string& profile)
 {
   profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile;
 }
-const std::string& CompositeInstruction::getProfile() const { return profile_; }
+const std::string& CompositeInstruction::getProfile(const std::string& ns) const
+{
+  if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end()))
+    return profile_;
+
+  return profile_overrides_.at(ns);
+}
 
-void CompositeInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides)
+void CompositeInstruction::setProfileOverrides(ProfileOverrides profile_overrides)
 {
   profile_overrides_ = std::move(profile_overrides);
 }
-ProfileDictionary::ConstPtr CompositeInstruction::getProfileOverrides() const { return profile_overrides_; }
+ProfileOverrides CompositeInstruction::getProfileOverrides() const { return profile_overrides_; }
 
 void CompositeInstruction::setManipulatorInfo(tesseract_common::ManipulatorInfo info)
 {

diff --git a/tesseract_command_language/src/move_instruction.cpp b/tesseract_command_language/src/move_instruction.cpp
@@ -148,22 +148,34 @@ const tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() c
 tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() { return manipulator_info_; }
 
 void MoveInstruction::setProfile(const std::string& profile) { profile_ = profile; }
-const std::string& MoveInstruction::getProfile() const { return profile_; }
+const std::string& MoveInstruction::getProfile(const std::string& ns) const
+{
+  if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end()))
+    return profile_;
+
+  return profile_overrides_.at(ns);
+}
 
 void MoveInstruction::setPathProfile(const std::string& profile) { path_profile_ = profile; }
-const std::string& MoveInstruction::getPathProfile() const { return path_profile_; }
+const std::string& MoveInstruction::getPathProfile(const std::string& ns) const
+{
+  if (ns.empty() || (path_profile_overrides_.find(ns) == path_profile_overrides_.end()))
+    return path_profile_;
+
+  return path_profile_overrides_.at(ns);
+}
 
-void MoveInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides)
+void MoveInstruction::setProfileOverrides(ProfileOverrides profile_overrides)
 {
   profile_overrides_ = std::move(profile_overrides);
 }
-ProfileDictionary::ConstPtr MoveInstruction::getProfileOverrides() const { return profile_overrides_; }
+ProfileOverrides MoveInstruction::getProfileOverrides() const { return profile_overrides_; }
 
-void MoveInstruction::setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides)
+void MoveInstruction::setPathProfileOverrides(ProfileOverrides profile_overrides)
 {
   path_profile_overrides_ = std::move(profile_overrides);
 }
-ProfileDictionary::ConstPtr MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; }
+ProfileOverrides MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; }
 
 const std::string& MoveInstruction::getDescription() const { return description_; }