Add Remap Task (#351)

tesseract_task_composer/README.rst

@@ -175,7 +175,8 @@ Define the graph nodes and edges as shown in the config below.
            destinations: [ErrorTask, DoneTask]
        terminals: [ErrorTask, DoneTask]
 
-Leveraging a perviously defined task.
+Leveraging a perviously defined task
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 When using a perviously defined task it is referenced using `task:` instead of `class:`. 
 
@@ -465,7 +466,7 @@ The final task that is called in a task graph if error occurs
      config:
        conditional: false
 
-Error Task
+Abort Task
 ^^^^^^^^^^
 
 The task that is called if you want to abort everything
@@ -477,6 +478,22 @@ The task that is called if you want to abort everything
      config:
        conditional: false
 
+Remap Task
+^^^^^^^^^^
+
+Remap data from one key to another, by copying or moving the data.
+
+.. code-block:: yaml
+
+   RemapTask:
+     class: RemapTaskFactory
+     config:
+       conditional: false
+       copy: true
+       remap:
+         key1: remap_key1
+         key2: remap_key2
+
 Fix State Bounds Task
 ^^^^^^^^^^^^^^^^^^^^^
 

tesseract_task_composer/core/CMakeLists.txt

@@ -41,6 +41,7 @@ add_library(
   src/nodes/abort_task.cpp
   src/nodes/done_task.cpp
   src/nodes/error_task.cpp
+  src/nodes/remap_task.cpp
   src/nodes/start_task.cpp
   src/test_suite/test_task.cpp)
 target_link_libraries(

tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h

@@ -0,0 +1,76 @@
+/**
+ * @file remap_task.h
+ *
+ * @author Levi Armstrong
+ * @date July 13, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, Levi Armstrong
+ *
+ * @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_TASK_COMPOSER_REMAP_TASK_H
+#define TESSERACT_TASK_COMPOSER_REMAP_TASK_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/map.hpp>
+#include <map>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/core/task_composer_task.h>
+
+namespace tesseract_planning
+{
+class TaskComposerPluginFactory;
+class RemapTask : public TaskComposerTask
+{
+public:
+  using Ptr = std::shared_ptr<RemapTask>;
+  using ConstPtr = std::shared_ptr<const RemapTask>;
+  using UPtr = std::unique_ptr<RemapTask>;
+  using ConstUPtr = std::unique_ptr<const RemapTask>;
+
+  RemapTask();
+  explicit RemapTask(std::string name,
+                     std::map<std::string, std::string> remap,
+                     bool copy = false,
+                     bool is_conditional = false);
+  explicit RemapTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& plugin_factory);
+  ~RemapTask() override = default;
+
+  bool operator==(const RemapTask& rhs) const;
+  bool operator!=(const RemapTask& rhs) const;
+
+protected:
+  std::map<std::string, std::string> remap_;
+  bool copy_{ false };
+
+  friend struct tesseract_common::Serialization;
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version);  // NOLINT
+
+  TaskComposerNodeInfo::UPtr runImpl(TaskComposerInput& input,
+                                     OptionalTaskComposerExecutor executor = std::nullopt) const override final;
+};
+
+}  // namespace tesseract_planning
+
+#include <boost/serialization/export.hpp>
+BOOST_CLASS_EXPORT_KEY2(tesseract_planning::RemapTask, "RemapTask")
+#endif  // TESSERACT_TASK_COMPOSER_REMAP_TASK_H

tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_data_storage.h

@@ -87,6 +87,14 @@ class TaskComposerDataStorage
    */
   std::unordered_map<std::string, tesseract_common::AnyPoly> getData() const;
 
+  /**
+   * @brief Remap data from one key to another
+   * @param remapping The key value pairs to remap data from the first to the second
+   * @param copy Default behavior is not move the data, but if copy is desired set this to true
+   * @return True if successful, otherwise false
+   */
+  bool remapData(const std::map<std::string, std::string>& remapping, bool copy = false);
+
   bool operator==(const TaskComposerDataStorage& rhs) const;
   bool operator!=(const TaskComposerDataStorage& rhs) const;
 

tesseract_task_composer/core/src/nodes/remap_task.cpp

@@ -0,0 +1,101 @@
+/**
+ * @file remap_task.h
+ *
+ * @author Levi Armstrong
+ * @date July 13, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, Levi Armstrong
+ *
+ * @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.
+ */
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <console_bridge/console.h>
+#include <yaml-cpp/yaml.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/core/nodes/remap_task.h>
+
+namespace tesseract_planning
+{
+RemapTask::RemapTask() : TaskComposerTask("RemapTask", false) {}
+RemapTask::RemapTask(std::string name, std::map<std::string, std::string> remap, bool copy, bool is_conditional)
+  : TaskComposerTask(std::move(name), is_conditional), remap_(std::move(remap)), copy_(copy)
+{
+  if (remap_.empty())
+    throw std::runtime_error("RemapTask, remap should not be empty!");
+}
+RemapTask::RemapTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& /*plugin_factory*/)
+  : TaskComposerTask(std::move(name), config)
+{
+  if (!input_keys_.empty())
+    throw std::runtime_error("RemapTask, input_keys should be empty!");
+
+  if (!output_keys_.empty())
+    throw std::runtime_error("RemapTask, output_keys should be empty!");
+
+  if (YAML::Node n = config["remap"])
+    remap_ = n.as<std::map<std::string, std::string>>();
+  else
+    throw std::runtime_error("RemapTask missing config key: 'remap'");
+
+  if (YAML::Node n = config["copy"])
+    copy_ = n.as<bool>();
+}
+
+TaskComposerNodeInfo::UPtr RemapTask::runImpl(TaskComposerInput& input, OptionalTaskComposerExecutor /*executor*/) const
+{
+  auto info = std::make_unique<TaskComposerNodeInfo>(*this);
+  if (input.data_storage.remapData(remap_, copy_))
+  {
+    info->color = "green";
+    info->return_value = 1;
+    info->message = "Successful";
+  }
+  else
+  {
+    info->color = "red";
+    info->return_value = 0;
+    info->message = "Failed to remap data.";
+  }
+  return info;
+}
+
+bool RemapTask::operator==(const RemapTask& rhs) const
+{
+  bool equal = true;
+  equal &= (remap_ == rhs.remap_);
+  equal &= (copy_ == rhs.copy_);
+  equal &= TaskComposerNode::operator==(rhs);
+  return equal;
+}
+bool RemapTask::operator!=(const RemapTask& rhs) const { return !operator==(rhs); }
+
+template <class Archive>
+void RemapTask::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask);
+  ar& boost::serialization::make_nvp("remap_data", remap_);
+  ar& boost::serialization::make_nvp("copy", copy_);
+}
+
+}  // namespace tesseract_planning
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RemapTask)
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RemapTask)

tesseract_task_composer/core/src/task_composer_data_storage.cpp

@@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #endif
 #include <boost/serialization/unordered_map.hpp>
 #include <mutex>
+#include <console_bridge/console.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_task_composer/core/task_composer_data_storage.h>
@@ -99,6 +100,48 @@ std::unordered_map<std::string, tesseract_common::AnyPoly> TaskComposerDataStora
   return data_;
 }
 
+bool TaskComposerDataStorage::remapData(const std::map<std::string, std::string>& remapping, bool copy)
+{
+  std::unique_lock lock(mutex_);
+
+  if (copy)
+  {
+    for (const auto& pair : remapping)
+    {
+      auto it = data_.find(pair.first);
+      if (it != data_.end())
+      {
+        data_[pair.second] = it->second;
+      }
+      else
+      {
+        CONSOLE_BRIDGE_logError(
+            "TaskComposerDataStorage, unable to remap data '%s' to '%s'", pair.first.c_str(), pair.second.c_str());
+        return false;
+      }
+    }
+  }
+  else
+  {
+    for (const auto& pair : remapping)
+    {
+      if (auto nh = data_.extract(pair.first); !nh.empty())
+      {
+        nh.key() = pair.second;
+        data_.insert(std::move(nh));
+      }
+      else
+      {
+        CONSOLE_BRIDGE_logError(
+            "TaskComposerDataStorage, unable to remap data '%s' to '%s'", pair.first.c_str(), pair.second.c_str());
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
+
 bool TaskComposerDataStorage::operator==(const TaskComposerDataStorage& rhs) const
 {
   std::shared_lock lhs_lock(mutex_, std::defer_lock);

tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp

@@ -32,6 +32,7 @@
 #include <tesseract_task_composer/core/nodes/abort_task.h>
 #include <tesseract_task_composer/core/nodes/done_task.h>
 #include <tesseract_task_composer/core/nodes/error_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/test_suite/test_task.h>
 
@@ -40,6 +41,7 @@ namespace tesseract_planning
 using AbortTaskFactory = TaskComposerTaskFactory<AbortTask>;
 using DoneTaskFactory = TaskComposerTaskFactory<DoneTask>;
 using ErrorTaskFactory = TaskComposerTaskFactory<ErrorTask>;
+using RemapTaskFactory = TaskComposerTaskFactory<RemapTask>;
 using StartTaskFactory = TaskComposerTaskFactory<StartTask>;
 using GraphTaskFactory = TaskComposerTaskFactory<TaskComposerGraph>;
 using PipelineTaskFactory = TaskComposerTaskFactory<TaskComposerPipeline>;
@@ -62,6 +64,8 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::DoneTaskFactory, Don
 // 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::RemapTaskFactory, RemapTaskFactory)
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::StartTaskFactory, StartTaskFactory)
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::GraphTaskFactory, GraphTaskFactory)