diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h index 47f0a70711..739b4b670f 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h @@ -45,9 +45,12 @@ class RemapTask : public TaskComposerTask using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; + // Requried + static const std::string INOUT_KEYS_PORT; + RemapTask(); explicit RemapTask(std::string name, - std::map remap, + const std::map& remap, bool copy = false, bool is_conditional = false); explicit RemapTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& plugin_factory); @@ -57,7 +60,6 @@ class RemapTask : public TaskComposerTask bool operator!=(const RemapTask& rhs) const; protected: - std::map remap_; bool copy_{ false }; friend struct tesseract_common::Serialization; @@ -65,6 +67,8 @@ class RemapTask : public TaskComposerTask template void serialize(Archive& ar, const unsigned int version); // NOLINT + static TaskComposerNodePorts ports(); + std::unique_ptr runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; }; diff --git a/tesseract_task_composer/core/src/nodes/remap_task.cpp b/tesseract_task_composer/core/src/nodes/remap_task.cpp index 9cda88d8c6..15e033eec9 100644 --- a/tesseract_task_composer/core/src/nodes/remap_task.cpp +++ b/tesseract_task_composer/core/src/nodes/remap_task.cpp @@ -38,30 +38,59 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -RemapTask::RemapTask() : TaskComposerTask("RemapTask", TaskComposerNodePorts{}, false) {} -RemapTask::RemapTask(std::string name, std::map remap, bool copy, bool is_conditional) - : TaskComposerTask(std::move(name), TaskComposerNodePorts{}, is_conditional), remap_(std::move(remap)), copy_(copy) +const std::string RemapTask::INOUT_KEYS_PORT = "keys"; + +RemapTask::RemapTask() : TaskComposerTask("RemapTask", RemapTask::ports(), false) {} +RemapTask::RemapTask(std::string name, const std::map& remap, bool copy, bool is_conditional) + : TaskComposerTask(std::move(name), RemapTask::ports(), is_conditional), copy_(copy) { - if (remap_.empty()) + if (remap.empty()) throw std::runtime_error("RemapTask, remap should not be empty!"); + + std::vector ikeys; + std::vector okeys; + ikeys.reserve(remap.size()); + okeys.reserve(remap.size()); + for (const auto& pair : remap) + { + ikeys.push_back(pair.first); + okeys.push_back(pair.second); + } + + input_keys_.add(INOUT_KEYS_PORT, ikeys); + output_keys_.add(INOUT_KEYS_PORT, okeys); + validatePorts(); } RemapTask::RemapTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& /*plugin_factory*/) - : TaskComposerTask(std::move(name), TaskComposerNodePorts{}, config) + : TaskComposerTask(std::move(name), RemapTask::ports(), config) { - if (YAML::Node n = config["remap"]) - remap_ = n.as>(); - else - throw std::runtime_error("RemapTask missing config key: 'remap'"); + if (input_keys_.get>(INOUT_KEYS_PORT).size() != + output_keys_.get>(INOUT_KEYS_PORT).size()) + throw std::runtime_error("RemapTask, input and ouput port 'keys' must be same size"); if (YAML::Node n = config["copy"]) copy_ = n.as(); } +TaskComposerNodePorts RemapTask::ports() +{ + TaskComposerNodePorts ports; + ports.input_required[INOUT_KEYS_PORT] = TaskComposerNodePorts::MULTIPLE; + ports.output_required[INOUT_KEYS_PORT] = TaskComposerNodePorts::MULTIPLE; + return ports; +} + std::unique_ptr RemapTask::runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor /*executor*/) const { auto info = std::make_unique(*this); - if (context.data_storage->remapData(remap_, copy_)) + const auto& ikeys = input_keys_.get>(INOUT_KEYS_PORT); + const auto& okeys = output_keys_.get>(INOUT_KEYS_PORT); + std::map remapping; + for (std::size_t i = 0; i < ikeys.size(); ++i) + remapping[ikeys[i]] = okeys[i]; + + if (context.data_storage->remapData(remapping, copy_)) { info->color = "green"; info->return_value = 1; @@ -81,9 +110,8 @@ std::unique_ptr RemapTask::runImpl(TaskComposerContext& co bool RemapTask::operator==(const RemapTask& rhs) const { bool equal = true; - equal &= (remap_ == rhs.remap_); equal &= (copy_ == rhs.copy_); - equal &= TaskComposerNode::operator==(rhs); + equal &= TaskComposerTask::operator==(rhs); return equal; } bool RemapTask::operator!=(const RemapTask& rhs) const { return !operator==(rhs); } @@ -92,7 +120,6 @@ template 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_); } diff --git a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp index e63ce5814e..58ef56d2b3 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -1732,8 +1732,10 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT std::string str = R"(config: conditional: true copy: true - remap: - test: test2)"; + inputs: + keys: [test] + outputs: + keys: [test2])"; YAML::Node config = YAML::Load(str); RemapTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); @@ -1811,8 +1813,10 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT std::string str = R"(config: conditional: true copy: true - remap: - joint_state: remap_joint_state)"; + inputs: + keys: [joint_state] + outputs: + keys: [remap_joint_state])"; YAML::Node config = YAML::Load(str); RemapTask task("RemapTaskTest", config["config"], factory); @@ -1840,8 +1844,10 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT std::string str = R"(config: conditional: true copy: false - remap: - joint_state: remap_joint_state)"; + inputs: + keys: [joint_state] + outputs: + keys: [remap_joint_state])"; YAML::Node config = YAML::Load(str); RemapTask task("RemapTaskTest", config["config"], factory); @@ -1874,14 +1880,14 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerRemapTaskTests) // NOLINT str = R"(config: conditional: true inputs: - port1: [input_data])"; + keys: [input_data])"; config = YAML::Load(str); EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT str = R"(config: conditional: true outputs: - port1: [output_data])"; + keys: [output_data])"; config = YAML::Load(str); EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT }