Skip to content

Commit

Permalink
Fix RemapTask support for global remapping
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Aug 14, 2024
1 parent 0776b5b commit 4895a8e
Show file tree
Hide file tree
Showing 3 changed files with 60 additions and 23 deletions.
8 changes: 6 additions & 2 deletions tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/remap_task.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,9 +45,12 @@ class RemapTask : public TaskComposerTask
using UPtr = std::unique_ptr<RemapTask>;
using ConstUPtr = std::unique_ptr<const RemapTask>;

// Requried
static const std::string INOUT_KEYS_PORT;

RemapTask();
explicit RemapTask(std::string name,
std::map<std::string, std::string> remap,
const 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);
Expand All @@ -57,14 +60,15 @@ class RemapTask : public TaskComposerTask
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

static TaskComposerNodePorts ports();

std::unique_ptr<TaskComposerNodeInfo>
runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final;
};
Expand Down
53 changes: 40 additions & 13 deletions tesseract_task_composer/core/src/nodes/remap_task.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string, std::string> 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<std::string, std::string>& 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<std::string> ikeys;
std::vector<std::string> 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<std::map<std::string, std::string>>();
else
throw std::runtime_error("RemapTask missing config key: 'remap'");
if (input_keys_.get<std::vector<std::string>>(INOUT_KEYS_PORT).size() !=
output_keys_.get<std::vector<std::string>>(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<bool>();
}

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<TaskComposerNodeInfo> RemapTask::runImpl(TaskComposerContext& context,
OptionalTaskComposerExecutor /*executor*/) const
{
auto info = std::make_unique<TaskComposerNodeInfo>(*this);
if (context.data_storage->remapData(remap_, copy_))
const auto& ikeys = input_keys_.get<std::vector<std::string>>(INOUT_KEYS_PORT);
const auto& okeys = output_keys_.get<std::vector<std::string>>(INOUT_KEYS_PORT);
std::map<std::string, std::string> 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;
Expand All @@ -81,9 +110,8 @@ std::unique_ptr<TaskComposerNodeInfo> 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); }
Expand All @@ -92,7 +120,6 @@ 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_);
}

Expand Down
22 changes: 14 additions & 8 deletions tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<RemapTask>("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<RemapTask>("abc", config["config"], factory)); // NOLINT
}
Expand Down

0 comments on commit 4895a8e

Please sign in to comment.