Add data storage to task composer node info

tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h

@@ -41,6 +41,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_common/any_poly.h>
 
 #include <tesseract_task_composer/core/task_composer_keys.h>
+#include <tesseract_task_composer/core/task_composer_data_storage.h>
 
 namespace tesseract_planning
 {
@@ -57,7 +58,7 @@ class TaskComposerNodeInfo
 
   TaskComposerNodeInfo() = default;  // Required for serialization
   TaskComposerNodeInfo(const TaskComposerNode& node);
-  virtual ~TaskComposerNodeInfo() = default;
+  ~TaskComposerNodeInfo() = default;
   TaskComposerNodeInfo(const TaskComposerNodeInfo&) = default;
   TaskComposerNodeInfo& operator=(const TaskComposerNodeInfo&) = default;
   TaskComposerNodeInfo(TaskComposerNodeInfo&&) = default;
@@ -117,6 +118,9 @@ class TaskComposerNodeInfo
    */
   std::string dotgraph;
 
+  /** @brief This provides a location for task data may be stored */
+  TaskComposerDataStorage data_storage;
+
   bool operator==(const TaskComposerNodeInfo& rhs) const;
   bool operator!=(const TaskComposerNodeInfo& rhs) const;
 
@@ -126,12 +130,6 @@ class TaskComposerNodeInfo
    */
   bool isAborted() const;
 
-  /**
-   * @brief This should perform a deep copy
-   * @return A clone
-   */
-  virtual TaskComposerNodeInfo::UPtr clone() const;
-
 private:
   friend struct tesseract_common::Serialization;
   friend class boost::serialization::access;

tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_node_info_unit.hpp

@@ -51,7 +51,7 @@ void runTaskComposerNodeInfoTest()
     EXPECT_TRUE(node_info.parent_uuid.is_nil());
     EXPECT_EQ(node_info.color, "red");
     EXPECT_FALSE(node_info.isAborted());
-    EXPECT_EQ(node_info, *(node_info.clone()));
+    EXPECT_EQ(node_info, TaskComposerNodeInfo(node_info));
 
     // Serialization
     test_suite::runSerializationTest<T>(node_info, "TaskComposerNodeInfoTests");
@@ -68,7 +68,7 @@ void runTaskComposerNodeInfoTest()
     EXPECT_EQ(node_info.parent_uuid, node.getParentUUID());
     EXPECT_EQ(node_info.color, "red");
     EXPECT_FALSE(node_info.isAborted());
-    EXPECT_EQ(node_info, *(node_info.clone()));
+    EXPECT_EQ(node_info, TaskComposerNodeInfo(node_info));
 
     // Serialization
     test_suite::runSerializationTest<T>(node_info, "TaskComposerNodeInfoTests");

tesseract_task_composer/core/include/tesseract_task_composer/core/test_suite/task_composer_serialization_utils.hpp

@@ -24,6 +24,15 @@ inline void runSerializationTest(const T& input, const std::string& file_name)
   EXPECT_FALSE(input != ninput);
 }
 
+template <typename T>
+inline void runSerializationTestNotEqual(const T& input, const std::string& file_name)
+{
+  const std::string filepath = tesseract_common::getTempPath() + file_name + ".xml";
+  tesseract_common::Serialization::toArchiveFileXML<T>(input, filepath);
+  auto ninput = tesseract_common::Serialization::fromArchiveFileXML<T>(filepath);
+  EXPECT_TRUE(input != ninput);
+}
+
 template <typename T>
 inline void runSerializationPointerTest(const T& input, const std::string& file_name)
 {

tesseract_task_composer/core/src/task_composer_node_info.cpp

@@ -73,6 +73,7 @@ bool TaskComposerNodeInfo::operator==(const TaskComposerNodeInfo& rhs) const
   equal &= output_keys == rhs.output_keys;
   equal &= color == rhs.color;
   equal &= dotgraph == rhs.dotgraph;
+  equal &= data_storage == rhs.data_storage;
   equal &= aborted_ == rhs.aborted_;
   return equal;
 }
@@ -81,8 +82,6 @@ bool TaskComposerNodeInfo::operator!=(const TaskComposerNodeInfo& rhs) const { r
 
 bool TaskComposerNodeInfo::isAborted() const { return aborted_; }
 
-TaskComposerNodeInfo::UPtr TaskComposerNodeInfo::clone() const { return std::make_unique<TaskComposerNodeInfo>(*this); }
-
 template <class Archive>
 void TaskComposerNodeInfo::serialize(Archive& ar, const unsigned int /*version*/)
 {
@@ -102,6 +101,7 @@ void TaskComposerNodeInfo::serialize(Archive& ar, const unsigned int /*version*/
   ar& boost::serialization::make_nvp("output_keys", output_keys);
   ar& boost::serialization::make_nvp("color", color);
   ar& boost::serialization::make_nvp("dotgraph", dotgraph);
+  ar& boost::serialization::make_nvp("data_storage", data_storage);
   ar& boost::serialization::make_nvp("aborted", aborted_);
 }
 
@@ -113,7 +113,7 @@ TaskComposerNodeInfoContainer::TaskComposerNodeInfoContainer(const TaskComposerN
 
   aborting_node_ = other.aborting_node_;  // NOLINT(cppcoreguidelines-prefer-member-initializer)
   for (const auto& pair : other.info_map_)
-    info_map_[pair.first] = pair.second->clone();
+    info_map_[pair.first] = std::make_unique<TaskComposerNodeInfo>(*pair.second);
 }
 TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(const TaskComposerNodeInfoContainer& other)
 {
@@ -123,7 +123,7 @@ TaskComposerNodeInfoContainer& TaskComposerNodeInfoContainer::operator=(const Ta
 
   aborting_node_ = other.aborting_node_;  // NOLINT(cppcoreguidelines-prefer-member-initializer)
   for (const auto& pair : other.info_map_)
-    info_map_[pair.first] = pair.second->clone();
+    info_map_[pair.first] = std::make_unique<TaskComposerNodeInfo>(*pair.second);
 
   return *this;
 }
@@ -162,7 +162,7 @@ TaskComposerNodeInfo::UPtr TaskComposerNodeInfoContainer::getInfo(const boost::u
   if (it == info_map_.end())
     return nullptr;
 
-  return it->second->clone();
+  return std::make_unique<TaskComposerNodeInfo>(*it->second);
 }
 
 std::vector<TaskComposerNodeInfo::UPtr>
@@ -173,7 +173,7 @@ TaskComposerNodeInfoContainer::find(const std::function<bool(const TaskComposerN
   for (const auto& info : info_map_)
   {
     if (search_fn(*info.second))
-      results.push_back(info.second->clone());
+      results.push_back(std::make_unique<TaskComposerNodeInfo>(*info.second));
   }
   return results;
 }
@@ -210,7 +210,7 @@ std::map<boost::uuids::uuid, TaskComposerNodeInfo::UPtr> TaskComposerNodeInfoCon
   std::shared_lock<std::shared_mutex> lock(mutex_);
   std::map<boost::uuids::uuid, TaskComposerNodeInfo::UPtr> copy;
   for (const auto& pair : info_map_)
-    copy[pair.first] = pair.second->clone();
+    copy[pair.first] = std::make_unique<TaskComposerNodeInfo>(*pair.second);
 
   if (!aborting_node_.is_nil())
     updateParents(copy, aborting_node_);
@@ -224,7 +224,7 @@ void TaskComposerNodeInfoContainer::insertInfoMap(const TaskComposerNodeInfoCont
   std::shared_lock rhs_lock(container.mutex_, std::defer_lock);
   std::scoped_lock lock{ lhs_lock, rhs_lock };
   for (const auto& pair : container.info_map_)
-    info_map_[pair.first] = pair.second->clone();
+    info_map_[pair.first] = std::make_unique<TaskComposerNodeInfo>(*pair.second);
 }
 
 void TaskComposerNodeInfoContainer::mergeInfoMap(TaskComposerNodeInfoContainer&& container)

tesseract_task_composer/planning/CMakeLists.txt

@@ -22,7 +22,6 @@ set(LIB_SOURCE_FILES
     src/nodes/format_as_result_task.cpp
     src/nodes/profile_switch_task.cpp
     src/nodes/min_length_task.cpp
-    src/nodes/motion_planner_task_info.cpp
     src/nodes/process_planning_input_task.cpp
     src/nodes/update_end_state_task.cpp
     src/nodes/update_start_and_end_state_task.cpp

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h

@@ -90,32 +90,8 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT ContinuousContactCheckTask :
   runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final;
 };
 
-class ContinuousContactCheckTaskInfo : public TaskComposerNodeInfo
-{
-public:
-  using Ptr = std::shared_ptr<ContinuousContactCheckTaskInfo>;
-  using ConstPtr = std::shared_ptr<const ContinuousContactCheckTaskInfo>;
-  using UPtr = std::unique_ptr<ContinuousContactCheckTaskInfo>;
-  using ConstUPtr = std::unique_ptr<const ContinuousContactCheckTaskInfo>;
-
-  ContinuousContactCheckTaskInfo() = default;
-  ContinuousContactCheckTaskInfo(const TaskComposerNodeInfo& task);
-
-  std::shared_ptr<const tesseract_environment::Environment> env;
-  std::vector<tesseract_collision::ContactResultMap> contact_results;
-
-  std::unique_ptr<TaskComposerNodeInfo> clone() const override;
-
-  bool operator==(const ContinuousContactCheckTaskInfo& rhs) const;
-  bool operator!=(const ContinuousContactCheckTaskInfo& rhs) const;
-
-private:
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version);  // NOLINT
-};
 }  // namespace tesseract_planning
 
 BOOST_CLASS_EXPORT_KEY(tesseract_planning::ContinuousContactCheckTask)
-BOOST_CLASS_EXPORT_KEY(tesseract_planning::ContinuousContactCheckTaskInfo)
+
 #endif  // TESSERACT_TASK_COMPOSER_CONTINUOUS_CONTACT_CHECK_TASK_H

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h

@@ -90,33 +90,7 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT DiscreteContactCheckTask : p
   runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final;
 };
 
-class DiscreteContactCheckTaskInfo : public TaskComposerNodeInfo
-{
-public:
-  using Ptr = std::shared_ptr<DiscreteContactCheckTaskInfo>;
-  using ConstPtr = std::shared_ptr<const DiscreteContactCheckTaskInfo>;
-  using UPtr = std::unique_ptr<DiscreteContactCheckTaskInfo>;
-  using ConstUPtr = std::unique_ptr<const DiscreteContactCheckTaskInfo>;
-
-  DiscreteContactCheckTaskInfo() = default;
-  DiscreteContactCheckTaskInfo(const TaskComposerNodeInfo& task);
-
-  std::shared_ptr<const tesseract_environment::Environment> env;
-  std::vector<tesseract_collision::ContactResultMap> contact_results;
-
-  std::unique_ptr<TaskComposerNodeInfo> clone() const override;
-
-  bool operator==(const DiscreteContactCheckTaskInfo& rhs) const;
-  bool operator!=(const DiscreteContactCheckTaskInfo& rhs) const;
-
-private:
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version);  // NOLINT
-};
-
 }  // namespace tesseract_planning
 
 BOOST_CLASS_EXPORT_KEY(tesseract_planning::DiscreteContactCheckTask)
-BOOST_CLASS_EXPORT_KEY(tesseract_planning::DiscreteContactCheckTaskInfo)
 #endif  // TESSERACT_TASK_COMPOSER_DISCRETE_CONTACT_CHECK_TASK_H

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h

@@ -99,31 +99,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT FixStateCollisionTask : publ
   runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final;
 };
 
-class FixStateCollisionTaskInfo : public TaskComposerNodeInfo
-{
-public:
-  using Ptr = std::shared_ptr<FixStateCollisionTaskInfo>;
-  using ConstPtr = std::shared_ptr<const FixStateCollisionTaskInfo>;
-  using UPtr = std::unique_ptr<FixStateCollisionTaskInfo>;
-  using ConstUPtr = std::unique_ptr<const FixStateCollisionTaskInfo>;
-
-  FixStateCollisionTaskInfo() = default;
-  FixStateCollisionTaskInfo(const FixStateCollisionTask& task);
-
-  std::unique_ptr<TaskComposerNodeInfo> clone() const override;
-
-  std::shared_ptr<const tesseract_environment::Environment> env;
-  std::vector<tesseract_collision::ContactResultMap> contact_results;
-
-  bool operator==(const FixStateCollisionTaskInfo& rhs) const;
-  bool operator!=(const FixStateCollisionTaskInfo& rhs) const;
-
-private:
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version);  // NOLINT
-};
-
 /**
  * @brief Checks if a joint state is in collision
  * @param start_pos Vector that represents a joint state
@@ -180,5 +155,4 @@ bool applyCorrectionWorkflow(WaypointPoly& waypoint,
 }  // namespace tesseract_planning
 
 BOOST_CLASS_EXPORT_KEY(tesseract_planning::FixStateCollisionTask)
-BOOST_CLASS_EXPORT_KEY(tesseract_planning::FixStateCollisionTaskInfo)
 #endif  // TESSERACT_TASK_COMPOSER_FIX_STATE_COLLISION_TASK_H

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp

@@ -35,7 +35,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_task_composer/core/task_composer_task.h>
-#include <tesseract_task_composer/planning/nodes/motion_planner_task_info.h>
 
 #include <tesseract_task_composer/core/task_composer_context.h>
 #include <tesseract_task_composer/core/task_composer_node_info.h>
@@ -141,7 +140,7 @@ class MotionPlannerTask : public TaskComposerTask
   std::unique_ptr<TaskComposerNodeInfo> runImpl(TaskComposerContext& context,
                                                 OptionalTaskComposerExecutor /*executor*/ = std::nullopt) const override
   {
-    auto info = std::make_unique<MotionPlannerTaskInfo>(*this);
+    auto info = std::make_unique<TaskComposerNodeInfo>(*this);
     info->return_value = 0;
     info->status_code = 0;
 
@@ -158,8 +157,9 @@ class MotionPlannerTask : public TaskComposerTask
       return info;
     }
 
-    auto env = env_poly.template as<std::shared_ptr<const tesseract_environment::Environment>>();
-    info->env = env;
+    std::shared_ptr<const tesseract_environment::Environment> env =
+        env_poly.template as<std::shared_ptr<const tesseract_environment::Environment>>()->clone();
+    info->data_storage.setData("environment", env);
 
     auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
     if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h

@@ -89,32 +89,7 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT TimeOptimalParameterizationT
   runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final;
 };
 
-class TimeOptimalParameterizationTaskInfo : public TaskComposerNodeInfo
-{
-public:
-  using Ptr = std::shared_ptr<TimeOptimalParameterizationTaskInfo>;
-  using ConstPtr = std::shared_ptr<const TimeOptimalParameterizationTaskInfo>;
-  using UPtr = std::unique_ptr<TimeOptimalParameterizationTaskInfo>;
-  using ConstUPtr = std::unique_ptr<const TimeOptimalParameterizationTaskInfo>;
-
-  TimeOptimalParameterizationTaskInfo() = default;
-  TimeOptimalParameterizationTaskInfo(const TimeOptimalParameterizationTask& task);
-
-  std::unique_ptr<TaskComposerNodeInfo> clone() const override;
-
-  bool operator==(const TimeOptimalParameterizationTaskInfo& rhs) const;
-  bool operator!=(const TimeOptimalParameterizationTaskInfo& rhs) const;
-
-  double max_velocity_scaling_factor{ 0 };
-  double max_acceleration_scaling_factor{ 0 };
-
-private:
-  friend class boost::serialization::access;
-  template <class Archive>
-  void serialize(Archive& ar, const unsigned int version);  // NOLINT
-};
 }  // namespace tesseract_planning
 
 BOOST_CLASS_EXPORT_KEY(tesseract_planning::TimeOptimalParameterizationTask)
-BOOST_CLASS_EXPORT_KEY(tesseract_planning::TimeOptimalParameterizationTaskInfo)
 #endif  // TESSERACT_TASK_COMPOSER_TIME_OPTIMAL_TRAJECTORY_GENERATION_TASK_H