Fix Raster and RasterOnly Tasks

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
 ^^^^^^^^^^
 

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

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/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/serialization/access.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/core/task_composer_task.h>
+
+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<SyncTask>;
+  using ConstPtr = std::shared_ptr<const SyncTask>;
+  using UPtr = std::unique_ptr<SyncTask>;
+  using ConstUPtr = std::unique_ptr<const SyncTask>;
+
+  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 <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::SyncTask, "SyncTask")
+
+#endif  // TESSERACT_TASK_COMPOSER_SYNC_TASK_H

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/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <console_bridge/console.h>
+#include <boost/serialization/string.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_task_composer/core/nodes/sync_task.h>
+
+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<TaskComposerNodeInfo>(*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 <class Archive>
+void SyncTask::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask);
+}
+
+}  // namespace tesseract_planning
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SyncTask)
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SyncTask)

tesseract_task_composer/core/src/task_composer_task_plugin_factory.cpp

@@ -34,6 +34,7 @@
 #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/nodes/sync_task.h>
 #include <tesseract_task_composer/core/test_suite/test_task.h>
 
 namespace tesseract_planning
@@ -43,6 +44,7 @@ using DoneTaskFactory = TaskComposerTaskFactory<DoneTask>;
 using ErrorTaskFactory = TaskComposerTaskFactory<ErrorTask>;
 using RemapTaskFactory = TaskComposerTaskFactory<RemapTask>;
 using StartTaskFactory = TaskComposerTaskFactory<StartTask>;
+using SyncTaskFactory = TaskComposerTaskFactory<SyncTask>;
 using GraphTaskFactory = TaskComposerTaskFactory<TaskComposerGraph>;
 using PipelineTaskFactory = TaskComposerTaskFactory<TaskComposerPipeline>;
 
@@ -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)

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<StartTask>();
   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));
 

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<StartTask>();
   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));
 

tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp

@@ -23,6 +23,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #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/nodes/sync_task.h>
 #include <tesseract_task_composer/core/test_suite/test_task.h>
 
 TESSERACT_ANY_EXPORT(tesseract_common, JointState)
@@ -1900,7 +1901,7 @@ TEST(TesseractTaskComposerCoreUnit, TaskComposerStartTaskTests)  // NOLINT
     auto task = std::make_unique<StartTask>("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<SyncTask>("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<SyncTask>("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<SyncTask>("abc", config["config"], factory));  // NOLINT
+  }
+
+  {  // Serialization
+    auto task = std::make_unique<SyncTask>("abc");
+
+    // Serialization
+    test_suite::runSerializationPointerTest(task, "TaskComposerSyncTaskTests");
+  }
+
+  {  // Test run method
+    auto input = std::make_unique<TaskComposerInput>(std::make_unique<TaskComposerProblem>());
+    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:

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