Add HasDataStorageEntryTask and FormatAsResultTask with unit tests (#462

)

tesseract_task_composer/core/CMakeLists.txt

@@ -41,6 +41,7 @@ add_library(
   ${PROJECT_NAME}_nodes
   src/nodes/done_task.cpp
   src/nodes/error_task.cpp
+  src/nodes/has_data_storage_entry_task.cpp
   src/nodes/remap_task.cpp
   src/nodes/start_task.cpp
   src/nodes/sync_task.cpp

tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/has_data_storage_entry_task.h

@@ -0,0 +1,49 @@
+#ifndef TESSERACT_TASK_COMPOSER_HAS_DATA_STORAGE_ENTRY_TASK_H
+#define TESSERACT_TASK_COMPOSER_HAS_DATA_STORAGE_ENTRY_TASK_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/export.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/core/task_composer_task.h>
+
+namespace tesseract_planning
+{
+class TaskComposerPluginFactory;
+class HasDataStorageEntryTask : public TaskComposerTask
+{
+public:
+  using Ptr = std::shared_ptr<HasDataStorageEntryTask>;
+  using ConstPtr = std::shared_ptr<const HasDataStorageEntryTask>;
+  using UPtr = std::unique_ptr<HasDataStorageEntryTask>;
+  using ConstUPtr = std::unique_ptr<const HasDataStorageEntryTask>;
+
+  HasDataStorageEntryTask();
+  explicit HasDataStorageEntryTask(std::string name,
+                                   const std::vector<std::string>& input_keys,
+                                   bool is_conditional = true);
+  explicit HasDataStorageEntryTask(std::string name,
+                                   const YAML::Node& config,
+                                   const TaskComposerPluginFactory& plugin_factory);
+  ~HasDataStorageEntryTask() override = default;
+
+  bool operator==(const HasDataStorageEntryTask& rhs) const;
+  bool operator!=(const HasDataStorageEntryTask& rhs) const;
+
+protected:
+  friend struct tesseract_common::Serialization;
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version);  // NOLINT
+
+  std::unique_ptr<TaskComposerNodeInfo>
+  runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final;
+};
+
+}  // namespace tesseract_planning
+
+BOOST_CLASS_EXPORT_KEY2(tesseract_planning::HasDataStorageEntryTask, "HasDataStorageEntryTask")
+
+#endif  // TESSERACT_TASK_COMPOSER_HAS_DATA_STORAGE_ENTRY_TASK_H

tesseract_task_composer/core/src/nodes/has_data_storage_entry_task.cpp

@@ -0,0 +1,76 @@
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <console_bridge/console.h>
+#include <boost/serialization/map.hpp>
+#include <yaml-cpp/yaml.h>
+#include <tesseract_common/serialization.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/core/nodes/has_data_storage_entry_task.h>
+#include <tesseract_task_composer/core/task_composer_context.h>
+#include <tesseract_task_composer/core/task_composer_data_storage.h>
+#include <tesseract_task_composer/core/task_composer_node_info.h>
+
+namespace tesseract_planning
+{
+HasDataStorageEntryTask::HasDataStorageEntryTask() : TaskComposerTask("HasDataStorageEntryTask", true) {}
+HasDataStorageEntryTask::HasDataStorageEntryTask(std::string name,
+                                                 const std::vector<std::string>& input_keys,
+                                                 bool is_conditional)
+  : TaskComposerTask(std::move(name), is_conditional)
+{
+  input_keys_ = input_keys;
+  if (input_keys_.empty())
+    throw std::runtime_error("HasDataStorageEntryTask, input_keys should not be empty!");
+}
+HasDataStorageEntryTask::HasDataStorageEntryTask(std::string name,
+                                                 const YAML::Node& config,
+                                                 const TaskComposerPluginFactory& /*plugin_factory*/)
+  : TaskComposerTask(std::move(name), config)
+{
+  if (input_keys_.empty())
+    throw std::runtime_error("HasDataStorageEntryTask, input_keys should not be empty!");
+
+  if (!output_keys_.empty())
+    throw std::runtime_error("HasDataStorageEntryTask, output_keys should be empty!");
+}
+
+std::unique_ptr<TaskComposerNodeInfo> HasDataStorageEntryTask::runImpl(TaskComposerContext& context,
+                                                                       OptionalTaskComposerExecutor /*executor*/) const
+{
+  auto info = std::make_unique<TaskComposerNodeInfo>(*this);
+  for (const auto& input_key : input_keys_)
+  {
+    if (!context.data_storage->hasKey(input_key))
+    {
+      info->color = "red";
+      info->return_value = 0;
+      info->status_code = 0;
+      info->status_message = "Missing input key";
+      return info;
+    }
+  }
+
+  info->color = "green";
+  info->return_value = 1;
+  info->status_code = 1;
+  info->status_message = "Successful";
+  return info;
+}
+
+bool HasDataStorageEntryTask::operator==(const HasDataStorageEntryTask& rhs) const
+{
+  return (TaskComposerNode::operator==(rhs));
+}
+bool HasDataStorageEntryTask::operator!=(const HasDataStorageEntryTask& rhs) const { return !operator==(rhs); }
+
+template <class Archive>
+void HasDataStorageEntryTask::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask);
+}
+
+}  // namespace tesseract_planning
+
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::HasDataStorageEntryTask)
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::HasDataStorageEntryTask)

tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp

@@ -31,6 +31,7 @@
 
 #include <tesseract_task_composer/core/nodes/done_task.h>
 #include <tesseract_task_composer/core/nodes/error_task.h>
+#include <tesseract_task_composer/core/nodes/has_data_storage_entry_task.h>
 #include <tesseract_task_composer/core/nodes/remap_task.h>
 #include <tesseract_task_composer/core/nodes/start_task.h>
 #include <tesseract_task_composer/core/nodes/sync_task.h>
@@ -40,6 +41,7 @@ namespace tesseract_planning
 {
 using DoneTaskFactory = TaskComposerTaskFactory<DoneTask>;
 using ErrorTaskFactory = TaskComposerTaskFactory<ErrorTask>;
+using HasDataStorageEntryTaskFactory = TaskComposerTaskFactory<HasDataStorageEntryTask>;
 using RemapTaskFactory = TaskComposerTaskFactory<RemapTask>;
 using StartTaskFactory = TaskComposerTaskFactory<StartTask>;
 using SyncTaskFactory = TaskComposerTaskFactory<SyncTask>;
@@ -57,11 +59,14 @@ namespace tesseract_planning::test_suite
 using TestTaskFactory = TaskComposerTaskFactory<TestTask>;
 }
 
+// clang-format off
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::DoneTaskFactory, DoneTaskFactory)
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::ErrorTaskFactory, ErrorTaskFactory)
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::HasDataStorageEntryTaskFactory, HasDataStorageEntryTaskFactory)
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::RemapTaskFactory, RemapTaskFactory)
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::StartTaskFactory, StartTaskFactory)
@@ -73,3 +78,4 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::GraphTaskFactory, Gr
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::PipelineTaskFactory, PipelineTaskFactory)
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::test_suite::TestTaskFactory, TestTaskFactory)
+// clang-format on

tesseract_task_composer/planning/CMakeLists.txt

@@ -40,6 +40,7 @@ set(LIB_SOURCE_FILES
     src/nodes/fix_state_bounds_task.cpp
     src/nodes/fix_state_collision_task.cpp
     src/nodes/format_as_input_task.cpp
+    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

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_as_result_task.h

@@ -0,0 +1,50 @@
+#ifndef TESSERACT_TASK_COMPOSER_FORMAT_AS_RESULT_TASK_H
+#define TESSERACT_TASK_COMPOSER_FORMAT_AS_RESULT_TASK_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/export.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/core/task_composer_task.h>
+
+namespace tesseract_planning
+{
+class TaskComposerPluginFactory;
+class FormatAsResultTask : public TaskComposerTask
+{
+public:
+  using Ptr = std::shared_ptr<FormatAsResultTask>;
+  using ConstPtr = std::shared_ptr<const FormatAsResultTask>;
+  using UPtr = std::unique_ptr<FormatAsResultTask>;
+  using ConstUPtr = std::unique_ptr<const FormatAsResultTask>;
+
+  FormatAsResultTask();
+  explicit FormatAsResultTask(std::string name,
+                              const std::vector<std::string>& input_keys,
+                              const std::vector<std::string>& output_keys,
+                              bool is_conditional = true);
+  explicit FormatAsResultTask(std::string name,
+                              const YAML::Node& config,
+                              const TaskComposerPluginFactory& plugin_factory);
+  ~FormatAsResultTask() override = default;
+
+  bool operator==(const FormatAsResultTask& rhs) const;
+  bool operator!=(const FormatAsResultTask& rhs) const;
+
+protected:
+  friend struct tesseract_common::Serialization;
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version);  // NOLINT
+
+  std::unique_ptr<TaskComposerNodeInfo>
+  runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final;
+};
+
+}  // namespace tesseract_planning
+
+BOOST_CLASS_EXPORT_KEY2(tesseract_planning::FormatAsResultTask, "FormatAsResultTask")
+
+#endif  // TESSERACT_TASK_COMPOSER_FORMAT_AS_RESULT_TASK_H

tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp

@@ -33,6 +33,7 @@
 #include <tesseract_task_composer/planning/nodes/fix_state_bounds_task.h>
 #include <tesseract_task_composer/planning/nodes/fix_state_collision_task.h>
 #include <tesseract_task_composer/planning/nodes/format_as_input_task.h>
+#include <tesseract_task_composer/planning/nodes/format_as_result_task.h>
 #include <tesseract_task_composer/planning/nodes/min_length_task.h>
 #include <tesseract_task_composer/planning/nodes/profile_switch_task.h>
 #include <tesseract_task_composer/planning/nodes/upsample_trajectory_task.h>
@@ -51,6 +52,7 @@ using DiscreteContactCheckTaskFactory = TaskComposerTaskFactory<DiscreteContactC
 using FixStateBoundsTaskFactory = TaskComposerTaskFactory<FixStateBoundsTask>;
 using FixStateCollisionTaskFactory = TaskComposerTaskFactory<FixStateCollisionTask>;
 using FormatAsInputTaskFactory = TaskComposerTaskFactory<FormatAsInputTask>;
+using FormatAsResultTaskFactory = TaskComposerTaskFactory<FormatAsResultTask>;
 using MinLengthTaskFactory = TaskComposerTaskFactory<MinLengthTask>;
 using ProfileSwitchTaskFactory = TaskComposerTaskFactory<ProfileSwitchTask>;
 using UpsampleTrajectoryTaskFactory = TaskComposerTaskFactory<UpsampleTrajectoryTask>;
@@ -79,6 +81,8 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FixStateCollisionTas
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FormatAsInputTaskFactory, FormatAsInputTaskFactory)
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FormatAsResultTaskFactory, FormatAsResultTaskFactory)
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::MinLengthTaskFactory, MinLengthTaskFactory)
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::ProfileSwitchTaskFactory, ProfileSwitchTaskFactory)

tesseract_task_composer/planning/src/nodes/format_as_result_task.cpp

@@ -0,0 +1,104 @@
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <console_bridge/console.h>
+#include <boost/serialization/map.hpp>
+#include <yaml-cpp/yaml.h>
+#include <tesseract_common/serialization.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/planning/nodes/format_as_result_task.h>
+#include <tesseract_task_composer/core/task_composer_context.h>
+#include <tesseract_task_composer/core/task_composer_data_storage.h>
+#include <tesseract_task_composer/core/task_composer_node_info.h>
+#include <tesseract_command_language/composite_instruction.h>
+#include <tesseract_command_language/poly/move_instruction_poly.h>
+
+namespace tesseract_planning
+{
+FormatAsResultTask::FormatAsResultTask() : TaskComposerTask("FormatAsResultTask", true) {}
+
+FormatAsResultTask::FormatAsResultTask(std::string name,
+                                       const std::vector<std::string>& input_keys,
+                                       const std::vector<std::string>& output_keys,
+                                       bool is_conditional)
+  : TaskComposerTask(std::move(name), is_conditional)
+{
+  input_keys_ = input_keys;
+  output_keys_ = output_keys;
+
+  if (input_keys_.empty())
+    throw std::runtime_error("FormatAsResultTask, input_keys should not be empty!");
+
+  if (output_keys_.empty())
+    throw std::runtime_error("FormatAsResultTask, output_keys should not be empty!");
+
+  if (input_keys_.size() != output_keys_.size())
+    throw std::runtime_error("FormatAsResultTask, input_keys and output_keys be the same size!");
+}
+
+FormatAsResultTask::FormatAsResultTask(std::string name,
+                                       const YAML::Node& config,
+                                       const TaskComposerPluginFactory& /*plugin_factory*/)
+  : TaskComposerTask(std::move(name), config)
+{
+  if (input_keys_.empty())
+    throw std::runtime_error("FormatAsResultTask, input_keys should not be empty!");
+
+  if (output_keys_.empty())
+    throw std::runtime_error("FormatAsResultTask, output_keys should not be empty!");
+
+  if (input_keys_.size() != output_keys_.size())
+    throw std::runtime_error("FormatAsResultTask, input_keys and output_keys be the same size!");
+}
+
+std::unique_ptr<TaskComposerNodeInfo> FormatAsResultTask::runImpl(TaskComposerContext& context,
+                                                                  OptionalTaskComposerExecutor /*executor*/) const
+{
+  for (std::size_t i = 0; i < input_keys_.size(); ++i)
+  {
+    auto input_data = context.data_storage->getData(input_keys_[i]);
+    auto& ci = input_data.as<CompositeInstruction>();
+    std::vector<std::reference_wrapper<InstructionPoly>> instructions = ci.flatten(&moveFilter);
+    for (auto& instruction : instructions)
+    {
+      auto& mi = instruction.get().as<MoveInstructionPoly>();
+      if (mi.getWaypoint().isStateWaypoint())
+        continue;
+
+      if (!mi.getWaypoint().isJointWaypoint())
+        throw std::runtime_error("FormatAsResultTask, unsupported waypoint type!");
+
+      auto& jwp = mi.getWaypoint().as<JointWaypointPoly>();
+
+      // Convert to StateWaypoint
+      StateWaypointPoly swp = mi.createStateWaypoint();
+      swp.setName(jwp.getName());
+      swp.setNames(jwp.getNames());
+      swp.setPosition(jwp.getPosition());
+      mi.assignStateWaypoint(swp);
+    }
+
+    context.data_storage->setData(output_keys_[i], ci);
+  }
+
+  auto info = std::make_unique<TaskComposerNodeInfo>(*this);
+  info->color = "green";
+  info->return_value = 1;
+  info->status_code = 1;
+  info->status_message = "Successful";
+  return info;
+}
+
+bool FormatAsResultTask::operator==(const FormatAsResultTask& rhs) const { return (TaskComposerNode::operator==(rhs)); }
+bool FormatAsResultTask::operator!=(const FormatAsResultTask& rhs) const { return !operator==(rhs); }
+
+template <class Archive>
+void FormatAsResultTask::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask);
+}
+
+}  // namespace tesseract_planning
+
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FormatAsResultTask)
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FormatAsResultTask)