From be0de3166d954ed11e5f457e6671e518b359fde4 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 13 Aug 2023 19:38:15 -0500 Subject: [PATCH] Fix Raster and RasterOnly Tasks --- tesseract_task_composer/README.rst | 12 +++ tesseract_task_composer/core/CMakeLists.txt | 1 + .../core/nodes/sync_task.h | 73 +++++++++++++++++++ .../core/src/nodes/sync_task.cpp | 72 ++++++++++++++++++ .../src/task_composer_task_plugin_factory.cpp | 4 + .../planning/src/nodes/raster_motion_task.cpp | 3 + .../src/nodes/raster_only_motion_task.cpp | 3 + .../tesseract_task_composer_core_unit.cpp | 70 +++++++++++++++++- .../tesseract_task_composer_planning_unit.cpp | 12 +++ 9 files changed, 249 insertions(+), 1 deletion(-) create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h create mode 100644 tesseract_task_composer/core/src/nodes/sync_task.cpp diff --git a/tesseract_task_composer/README.rst b/tesseract_task_composer/README.rst index a56ca6bc09..ab3d3fb59c 100644 --- a/tesseract_task_composer/README.rst +++ b/tesseract_task_composer/README.rst @@ -478,6 +478,18 @@ The task that is called if you want to abort everything config: conditional: false +Sync Task +^^^^^^^^^ + +The task is used to create a syncronization point within a task graph + +.. code-block:: yaml + + SyncTask: + class: SyncTaskFactory + config: + conditional: false + Remap Task ^^^^^^^^^^ diff --git a/tesseract_task_composer/core/CMakeLists.txt b/tesseract_task_composer/core/CMakeLists.txt index 9fc107488c..7fbb9d1a4a 100644 --- a/tesseract_task_composer/core/CMakeLists.txt +++ b/tesseract_task_composer/core/CMakeLists.txt @@ -43,6 +43,7 @@ add_library( src/nodes/error_task.cpp src/nodes/remap_task.cpp src/nodes/start_task.cpp + src/nodes/sync_task.cpp src/test_suite/test_task.cpp) target_link_libraries( ${PROJECT_NAME}_nodes diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h new file mode 100644 index 0000000000..e0ae1ba606 --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/nodes/sync_task.h @@ -0,0 +1,73 @@ +/** + * @file sync_task.h + * + * @author Levi Armstrong + * @date August 13, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Plectix Robotics + * + * @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_SYNC_TASK_H +#define TESSERACT_TASK_COMPOSER_SYNC_TASK_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerPluginFactory; + +/** + * @brief This is used as a syncronization point in your task graph + * This task has no inputs or output and is not conditional + */ +class SyncTask : public TaskComposerTask +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + explicit SyncTask(std::string name = "SyncTask"); + explicit SyncTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& plugin_factory); + ~SyncTask() override = default; + + bool operator==(const SyncTask& rhs) const; + bool operator!=(const SyncTask& rhs) const; + +protected: + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + template + 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_CLASS_EXPORT_KEY2(tesseract_planning::SyncTask, "SyncTask") + +#endif // TESSERACT_TASK_COMPOSER_SYNC_TASK_H diff --git a/tesseract_task_composer/core/src/nodes/sync_task.cpp b/tesseract_task_composer/core/src/nodes/sync_task.cpp new file mode 100644 index 0000000000..5f51030411 --- /dev/null +++ b/tesseract_task_composer/core/src/nodes/sync_task.cpp @@ -0,0 +1,72 @@ +/** + * @file sync_task.cpp + * + * @author Levi Armstrong + * @date August 13, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Plectix Robotics + * + * @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_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +SyncTask::SyncTask(std::string name) : TaskComposerTask(std::move(name), false) {} +SyncTask::SyncTask(std::string name, const YAML::Node& config, const TaskComposerPluginFactory& /*plugin_factory*/) + : TaskComposerTask(std::move(name), config) +{ + if (conditional_) + throw std::runtime_error("SyncTask, config is_conditional should not be true"); + + if (!input_keys_.empty()) + throw std::runtime_error("SyncTask, config does not support 'inputs' entry"); + + if (!output_keys_.empty()) + throw std::runtime_error("SyncTask, config does not support 'outputs' entry"); +} +TaskComposerNodeInfo::UPtr SyncTask::runImpl(TaskComposerInput& /*input*/, + OptionalTaskComposerExecutor /*executor*/) const +{ + auto info = std::make_unique(*this); + info->color = "green"; + info->message = "Successful"; + info->return_value = 1; + return info; +} + +bool SyncTask::operator==(const SyncTask& rhs) const { return (TaskComposerTask::operator==(rhs)); } +bool SyncTask::operator!=(const SyncTask& rhs) const { return !operator==(rhs); } + +template +void SyncTask::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SyncTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SyncTask) diff --git a/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp b/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp index 8ad9b83927..aa597d347f 100644 --- a/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp +++ b/tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include namespace tesseract_planning @@ -43,6 +44,7 @@ using DoneTaskFactory = TaskComposerTaskFactory; using ErrorTaskFactory = TaskComposerTaskFactory; using RemapTaskFactory = TaskComposerTaskFactory; using StartTaskFactory = TaskComposerTaskFactory; +using SyncTaskFactory = TaskComposerTaskFactory; using GraphTaskFactory = TaskComposerTaskFactory; using PipelineTaskFactory = TaskComposerTaskFactory; @@ -68,6 +70,8 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::RemapTaskFactory, Re // 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::SyncTaskFactory, SyncTaskFactory) +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::GraphTaskFactory, GraphTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::PipelineTaskFactory, PipelineTaskFactory) diff --git a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp index 34d351f9d0..d0146e0065 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp @@ -326,6 +326,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, tesseract_common::ManipulatorInfo program_manip_info = program.getManipulatorInfo().getCombined(problem.manip_info); + // Start Task auto start_task = std::make_unique(); auto start_uuid = task_graph.addNode(std::move(start_task)); @@ -352,6 +353,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, const std::string task_name = "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription(); auto raster_results = raster_task_factory_(task_name, raster_idx + 1); + raster_results.node->setConditional(false); auto raster_uuid = task_graph.addNode(std::move(raster_results.node)); raster_tasks.emplace_back(raster_uuid, std::make_pair(raster_results.input_key, raster_results.output_key)); input.data_storage.setData(raster_results.input_key, raster_input); @@ -384,6 +386,7 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input, const std::string task_name = "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription(); auto transition_results = transition_task_factory_(task_name, transition_idx + 1); + transition_results.node->setConditional(false); auto transition_uuid = task_graph.addNode(std::move(transition_results.node)); transition_keys.emplace_back(std::make_pair(transition_results.input_key, transition_results.output_key)); diff --git a/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp index cab076a89d..6bb8523429 100644 --- a/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp @@ -276,6 +276,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu tesseract_common::ManipulatorInfo program_manip_info = program.getManipulatorInfo().getCombined(problem.manip_info); + // Start Task auto start_task = std::make_unique(); auto start_uuid = task_graph.addNode(std::move(start_task)); @@ -305,6 +306,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu const std::string task_name = "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription(); auto raster_results = raster_task_factory_(task_name, raster_idx + 1); + raster_results.node->setConditional(false); auto raster_uuid = task_graph.addNode(std::move(raster_results.node)); raster_tasks.emplace_back(raster_uuid, std::make_pair(raster_results.input_key, raster_results.output_key)); input.data_storage.setData(raster_results.input_key, raster_input); @@ -336,6 +338,7 @@ TaskComposerNodeInfo::UPtr RasterOnlyMotionTask::runImpl(TaskComposerInput& inpu const std::string task_name = "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription(); auto transition_results = transition_task_factory_(task_name, transition_idx + 1); + transition_results.node->setConditional(false); auto transition_uuid = task_graph.addNode(std::move(transition_results.node)); transition_keys.emplace_back(std::make_pair(transition_results.input_key, transition_results.output_key)); 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 c7cd62b0af..33e53841d4 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp @@ -23,6 +23,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include TESSERACT_ANY_EXPORT(tesseract_common, JointState) @@ -1900,7 +1901,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests) // NOLINT auto task = std::make_unique("abc"); // Serialization - test_suite::runSerializationPointerTest(task, "TaskComposerDoneTaskTests"); + test_suite::runSerializationPointerTest(task, "TaskComposerStartTaskTests"); } { // Test run method @@ -1918,6 +1919,73 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests) // NOLINT } } +TEST(TesseractTaskComposerCoreUnit, TaskComposerSyncTaskTests) // NOLINT +{ + { // Construction + SyncTask task; + EXPECT_EQ(task.getName(), "SyncTask"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: false)"; + YAML::Node config = YAML::Load(str); + SyncTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true)"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: false + inputs: [input_data] + ouputs: [output_data])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: false + outputs: [output_data])"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Serialization + auto task = std::make_unique("abc"); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerSyncTaskTests"); + } + + { // Test run method + auto input = std::make_unique(std::make_unique()); + SyncTask task; + EXPECT_EQ(task.run(*input), 1); + auto node_info = input->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "green"); + EXPECT_EQ(node_info->return_value, 1); + EXPECT_EQ(node_info->message, "Successful"); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(input->isAborted(), false); + EXPECT_EQ(input->isSuccessful(), true); + EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + } +} + TEST(TesseractTaskComposerCoreUnit, TaskComposerServerTests) // NOLINT { std::string str = R"(task_composer_plugins: diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index 871128146a..058e9f7c86 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -2564,10 +2564,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // TaskComposerFuture::UPtr future = executor->run(*task, *input); future->wait(); + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "TaskComposerRasterMotionTaskTests.dot"); + EXPECT_NO_THROW(task->dump(os1, nullptr, input->task_infos.getInfoMap())); // NOLINT + os1.close(); + EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); EXPECT_TRUE(input->data_storage.hasKey(output_key)); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(input->task_infos.getInfoMap().size(), 94); } { // Failure missing input data @@ -2986,10 +2992,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) TaskComposerFuture::UPtr future = executor->run(*task, *input); future->wait(); + std::ofstream os1; + os1.open(tesseract_common::getTempPath() + "TaskComposerRasterOnlyMotionTaskTests.dot"); + EXPECT_NO_THROW(task->dump(os1, nullptr, input->task_infos.getInfoMap())); // NOLINT + os1.close(); + EXPECT_EQ(input->isAborted(), false); EXPECT_EQ(input->isSuccessful(), true); EXPECT_TRUE(input->data_storage.hasKey(output_key)); EXPECT_TRUE(input->task_infos.getAbortingNode().is_nil()); + EXPECT_EQ(input->task_infos.getInfoMap().size(), 74); } { // Failure missing input data