Add user data support to CompositeInstruction

tesseract-robotics · Sep 26, 2023 · 1a19448 · 1a19448
1 parent 61591c7
commit 1a19448
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h
@@ -30,13 +30,15 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <vector>
 #include <string>
+#include <variant>
 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_common/manipulator_info.h>
+#include <tesseract_common/any_poly.h>
 
 namespace tesseract_planning
 {
@@ -69,6 +71,10 @@ class CompositeInstruction
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   // LCOV_EXCL_STOP
 
+  /** User Data */
+  using UserDataVariant = std::variant<double, long, std::size_t, std::string, tesseract_common::AnyPoly>;
+  using UserData = std::unordered_map<std::string, UserDataVariant>;
+
   /** value_type */
   using value_type = InstructionPoly;
   /** pointer */
@@ -241,6 +247,12 @@ class CompositeInstruction
    */
   std::vector<std::reference_wrapper<const InstructionPoly>> flatten(const flattenFilterFn& filter = nullptr) const;
 
+  /** @brief Get user data */
+  UserData& getUserData();
+
+  /** @brief Get user data (const) */
+  const UserData& getUserData() const;
+
   bool operator==(const CompositeInstruction& rhs) const;
 
   bool operator!=(const CompositeInstruction& rhs) const;
@@ -391,6 +403,9 @@ class CompositeInstruction
   /** @brief The order of the composite instruction */
   CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED };
 
+  /** @brief A container to store user data */
+  UserData user_data_;
+
   const InstructionPoly* getFirstInstructionHelper(const CompositeInstruction& composite_instruction,
                                                    const locateFilterFn& locate_filter,
                                                    bool process_child_composites) const;

diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp
@@ -30,6 +30,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <iostream>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/vector.hpp>
+#include <boost/serialization/unordered_map.hpp>
+#include <tesseract_common/std_variant_serialization.h>
 #include <console_bridge/console.h>
 #include <boost/uuid/uuid_generators.hpp>
 #include <boost/uuid/uuid_io.hpp>
@@ -196,6 +198,10 @@ CompositeInstruction::flatten(const flattenFilterFn& filter) const
   return flattened;
 }
 
+CompositeInstruction::UserData& CompositeInstruction::getUserData() { return user_data_; }
+
+const CompositeInstruction::UserData& CompositeInstruction::getUserData() const { return user_data_; }
+
 void CompositeInstruction::print(const std::string& prefix) const
 {
   std::cout << prefix + "Composite Instruction, Description: " << getDescription() << std::endl;
@@ -216,6 +222,7 @@ bool CompositeInstruction::operator==(const CompositeInstruction& rhs) const
   equal &= (static_cast<int>(order_) == static_cast<int>(rhs.order_));
   equal &= (profile_ == rhs.profile_);  // NOLINT
   equal &= (manipulator_info_ == rhs.manipulator_info_);
+  equal &= (user_data_ == rhs.user_data_);
   equal &= (container_.size() == rhs.container_.size());
   if (equal)
   {
@@ -517,6 +524,7 @@ void CompositeInstruction::serialize(Archive& ar, const unsigned int /*version*/
   ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_);
   ar& boost::serialization::make_nvp("profile", profile_);
   ar& boost::serialization::make_nvp("order", order_);
+  ar& boost::serialization::make_nvp("user_data", user_data_);
   ar& boost::serialization::make_nvp("container", container_);
 }