Modify the concept of profile overrides in Composite and Move Instruc…

…tion

tesseract_command_language/include/tesseract_command_language/composite_instruction.h

@@ -37,13 +37,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/poly/instruction_poly.h>
 #include <tesseract_command_language/constants.h>
+#include <tesseract_command_language/types.h>
 #include <tesseract_common/manipulator_info.h>
 #include <tesseract_common/any_poly.h>
 
 namespace tesseract_planning
 {
 class CompositeInstruction;
-class ProfileDictionary;
 struct MoveInstructionPoly;
 
 /**
@@ -134,10 +134,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(std::shared_ptr<const ProfileDictionary> profile_overrides);
-  std::shared_ptr<const ProfileDictionary> getProfileOverrides() const;
+  void setProfileOverrides(ProfileOverrides profile_overrides);
+  const ProfileOverrides& getProfileOverrides() const;
 
   void setManipulatorInfo(tesseract_common::ManipulatorInfo info);
   const tesseract_common::ManipulatorInfo& getManipulatorInfo() const;
@@ -409,7 +409,7 @@ class CompositeInstruction
   std::string profile_{ DEFAULT_PROFILE_KEY };
 
   /** @brief Dictionary of profiles that will override named profiles for a specific task*/
-  std::shared_ptr<const ProfileDictionary> profile_overrides_;
+  ProfileOverrides profile_overrides_;
 
   /** @brief The order of the composite instruction */
   CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED };

tesseract_command_language/include/tesseract_command_language/move_instruction.h

@@ -35,6 +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/types.h>
 #include <tesseract_common/manipulator_info.h>
 
 namespace tesseract_planning
@@ -139,16 +140,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(std::shared_ptr<const ProfileDictionary> profile_overrides);
-  std::shared_ptr<const ProfileDictionary> getProfileOverrides() const;
+  void setProfileOverrides(ProfileOverrides profile_overrides);
+  const ProfileOverrides& getProfileOverrides() const;
 
-  void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides);
-  std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const;
+  void setPathProfileOverrides(ProfileOverrides profile_overrides);
+  const ProfileOverrides& getPathProfileOverrides() const;
 
   const std::string& getDescription() const;
 
@@ -183,10 +184,10 @@ class MoveInstruction
   std::string path_profile_;
 
   /** @brief Dictionary of profiles that will override named profiles for a specific task*/
-  std::shared_ptr<const ProfileDictionary> profile_overrides_;
+  ProfileOverrides profile_overrides_;
 
   /** @brief Dictionary of path profiles that will override named profiles for a specific task*/
-  std::shared_ptr<const ProfileDictionary> path_profile_overrides_;
+  ProfileOverrides path_profile_overrides_;
 
   /** @brief The assigned waypoint (Cartesian, Joint or State) */
   WaypointPoly waypoint_;

tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h

@@ -40,6 +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/types.h>
 #include <tesseract_common/manipulator_info.h>
 #include <tesseract_common/type_erasure.h>
 #include <tesseract_common/fwd.h>
@@ -127,11 +128,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);
 
@@ -180,16 +181,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(std::shared_ptr<const ProfileDictionary> profile_overrides) = 0;
-  virtual std::shared_ptr<const ProfileDictionary> getProfileOverrides() const = 0;
+  virtual void setProfileOverrides(ProfileOverrides profile_overrides) = 0;
+  virtual const ProfileOverrides& getProfileOverrides() const = 0;
 
-  virtual void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides) = 0;
-  virtual std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const = 0;
+  virtual void setPathProfileOverrides(ProfileOverrides profile_overrides) = 0;
+  virtual const ProfileOverrides& getPathProfileOverrides() const = 0;
 
   virtual void setMoveType(MoveInstructionType move_type) = 0;
   virtual MoveInstructionType getMoveType() const = 0;
@@ -244,28 +245,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(std::shared_ptr<const ProfileDictionary> profile_overrides) final
+  void setProfileOverrides(ProfileOverrides profile_overrides) final
   {
-    this->get().setProfileOverrides(profile_overrides);
-  }
-  std::shared_ptr<const ProfileDictionary> getProfileOverrides() const final
-  {
-    return this->get().getProfileOverrides();
+    this->get().setProfileOverrides(std::move(profile_overrides));
   }
+  const ProfileOverrides& getProfileOverrides() const final { return this->get().getProfileOverrides(); }
 
-  void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides) final
-  {
-    this->get().setPathProfileOverrides(profile_overrides);
-  }
-  std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const final
+  void setPathProfileOverrides(ProfileOverrides profile_overrides) final
   {
-    return this->get().getPathProfileOverrides();
+    this->get().setPathProfileOverrides(std::move(profile_overrides));
   }
+  const 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(); }
@@ -321,16 +316,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(std::shared_ptr<const ProfileDictionary> profile_overrides);
-  std::shared_ptr<const ProfileDictionary> getProfileOverrides() const;
+  void setProfileOverrides(ProfileOverrides profile_overrides);
+  const ProfileOverrides& getProfileOverrides() const;
 
-  void setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides);
-  std::shared_ptr<const ProfileDictionary> getPathProfileOverrides() const;
+  void setPathProfileOverrides(ProfileOverrides profile_overrides);
+  const ProfileOverrides& getPathProfileOverrides() const;
 
   void setMoveType(MoveInstructionType move_type);
   MoveInstructionType getMoveType() const;

tesseract_command_language/include/tesseract_command_language/types.h

@@ -0,0 +1,45 @@
+/**
+ * @file types.h
+ * @brief Contains common types used throughout command language
+ *
+ * @author Levi Armstrong
+ * @date July 22, 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_COMMAND_LANGUAGE_TYPES_H
+#define TESSERACT_COMMAND_LANGUAGE_TYPES_H
+
+#include <unordered_map>
+#include <string>
+
+namespace tesseract_planning
+{
+/**
+ * @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

tesseract_command_language/src/composite_instruction.cpp

@@ -82,16 +82,19 @@ void CompositeInstruction::setProfile(const std::string& profile)
 {
   profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile;
 }
-const std::string& CompositeInstruction::getProfile() const { return profile_; }
-
-void CompositeInstruction::setProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides)
+const std::string& CompositeInstruction::getProfile(const std::string& ns) const
 {
-  profile_overrides_ = std::move(profile_overrides);
+  if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end()))
+    return profile_;
+
+  return profile_overrides_.at(ns);
 }
-std::shared_ptr<const ProfileDictionary> CompositeInstruction::getProfileOverrides() const
+
+void CompositeInstruction::setProfileOverrides(ProfileOverrides profile_overrides)
 {
-  return profile_overrides_;
+  profile_overrides_ = std::move(profile_overrides);
 }
+const ProfileOverrides& CompositeInstruction::getProfileOverrides() const { return profile_overrides_; }
 
 void CompositeInstruction::setManipulatorInfo(tesseract_common::ManipulatorInfo info)
 {

tesseract_command_language/src/move_instruction.cpp

@@ -37,7 +37,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/cartesian_waypoint.h>
 #include <tesseract_command_language/joint_waypoint.h>
 #include <tesseract_command_language/state_waypoint.h>
-#include <tesseract_command_language/profile_dictionary.h>
 
 namespace tesseract_planning
 {
@@ -191,25 +190,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_;
 
-void MoveInstruction::setProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides)
+  return path_profile_overrides_.at(ns);
+}
+
+void MoveInstruction::setProfileOverrides(ProfileOverrides profile_overrides)
 {
   profile_overrides_ = std::move(profile_overrides);
 }
-std::shared_ptr<const ProfileDictionary> MoveInstruction::getProfileOverrides() const { return profile_overrides_; }
+const ProfileOverrides& MoveInstruction::getProfileOverrides() const { return profile_overrides_; }
 
-void MoveInstruction::setPathProfileOverrides(std::shared_ptr<const ProfileDictionary> profile_overrides)
+void MoveInstruction::setPathProfileOverrides(ProfileOverrides profile_overrides)
 {
   path_profile_overrides_ = std::move(profile_overrides);
 }
-std::shared_ptr<const ProfileDictionary> MoveInstruction::getPathProfileOverrides() const
-{
-  return path_profile_overrides_;
-}
+const ProfileOverrides& MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; }
 
 const std::string& MoveInstruction::getDescription() const { return description_; }
 

tesseract_command_language/src/poly/move_instruction_poly.cpp

@@ -71,35 +71,34 @@ void tesseract_planning::MoveInstructionPoly::setProfile(const std::string& prof
 {
   getInterface().setProfile(profile);
 }
-const std::string& tesseract_planning::MoveInstructionPoly::getProfile() const { return getInterface().getProfile(); }
+const std::string& tesseract_planning::MoveInstructionPoly::getProfile(const std::string& ns) const
+{
+  return getInterface().getProfile(ns);
+}
 
 void tesseract_planning::MoveInstructionPoly::setPathProfile(const std::string& profile)
 {
   getInterface().setPathProfile(profile);
 }
-const std::string& tesseract_planning::MoveInstructionPoly::getPathProfile() const
+const std::string& tesseract_planning::MoveInstructionPoly::getPathProfile(const std::string& ns) const
 {
-  return getInterface().getPathProfile();
+  return getInterface().getPathProfile(ns);
 }
 
-void tesseract_planning::MoveInstructionPoly::setProfileOverrides(
-    std::shared_ptr<const ProfileDictionary> profile_overrides)
+void tesseract_planning::MoveInstructionPoly::setProfileOverrides(ProfileOverrides profile_overrides)
 {
   getInterface().setProfileOverrides(std::move(profile_overrides));
 }
-std::shared_ptr<const tesseract_planning::ProfileDictionary>
-tesseract_planning::MoveInstructionPoly::getProfileOverrides() const
+const ProfileOverrides& tesseract_planning::MoveInstructionPoly::getProfileOverrides() const
 {
   return getInterface().getProfileOverrides();
 }
 
-void tesseract_planning::MoveInstructionPoly::setPathProfileOverrides(
-    std::shared_ptr<const ProfileDictionary> profile_overrides)
+void tesseract_planning::MoveInstructionPoly::setPathProfileOverrides(ProfileOverrides profile_overrides)
 {
   getInterface().setPathProfileOverrides(std::move(profile_overrides));
 }
-std::shared_ptr<const tesseract_planning::ProfileDictionary>
-tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const
+const ProfileOverrides& tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const
 {
   return getInterface().getPathProfileOverrides();
 }