Fix Raster and RasterOnly Tasks

Levi-Armstrong · Levi-Armstrong · commit 37434439a376 · 2023-08-15T10:16:06.000-05:00
diff --git 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
@@ -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
@@ -0,0 +1,78 @@
+/**
+ * @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 synchronization point in your task graph
+ *
+ * The actual synchronization is something that the executor will inherently do based on the graph structure but it
+ * needs a actual task so you can connect all parallel tasks to. This way when it gets to this task the executor will
+ * not proceed until all preceding tasks are finished.
+ *
+ * @note 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
diff --git a/tesseract_task_composer/core/src/nodes/sync_task.cpp 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/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)
diff --git a/tesseract_task_composer/core/src/task_composer_pipeline.cpp b/tesseract_task_composer/core/src/task_composer_pipeline.cpp
@@ -53,6 +53,8 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec
   if (input.isAborted())
   {
     auto info = std::make_unique<TaskComposerNodeInfo>(*this);
+    info->input_keys = input_keys_;
+    info->output_keys = output_keys_;
     info->return_value = 0;
     info->color = "white";
     info->message = "Aborted";
@@ -76,6 +78,8 @@ int TaskComposerPipeline::run(TaskComposerInput& input, OptionalTaskComposerExec
     results->return_value = 0;
   }
   timer.stop();
+  results->input_keys = input_keys_;
+  results->output_keys = output_keys_;
   results->elapsed_time = timer.elapsedSeconds();
 
   int value = results->return_value;
@@ -114,6 +118,8 @@ TaskComposerNodeInfo::UPtr TaskComposerPipeline::runImpl(TaskComposerInput& inpu
     {
       timer.stop();
       auto info = std::make_unique<TaskComposerNodeInfo>(*this);
+      info->input_keys = input_keys_;
+      info->output_keys = output_keys_;
       info->return_value = static_cast<int>(i);
       info->color = node_info->color;
       info->message = node_info->message;
diff --git a/tesseract_task_composer/core/src/task_composer_task.cpp b/tesseract_task_composer/core/src/task_composer_task.cpp
@@ -51,6 +51,8 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor
   if (input.isAborted())
   {
     auto info = std::make_unique<TaskComposerNodeInfo>(*this);
+    info->input_keys = input_keys_;
+    info->output_keys = output_keys_;
     info->return_value = 0;
     info->color = "white";
     info->message = "Aborted";
@@ -74,6 +76,8 @@ int TaskComposerTask::run(TaskComposerInput& input, OptionalTaskComposerExecutor
     results->return_value = 0;
   }
   timer.stop();
+  results->input_keys = input_keys_;
+  results->output_keys = output_keys_;
   results->elapsed_time = timer.elapsedSeconds();
 
   int value = results->return_value;
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
@@ -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)
diff --git 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<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,19 +386,23 @@ 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));
 
     const auto& prev = raster_tasks[transition_idx];
     const auto& next = raster_tasks[transition_idx + 1];
     const auto& prev_output = prev.second.second;
     const auto& next_output = next.second.second;
-    auto transition_mux_task = std::make_unique<UpdateStartAndEndStateTask>(
-        "UpdateStartAndEndStateTask", prev_output, next_output, transition_results.input_key, false);
-    std::string transition_mux_key = transition_mux_task->getUUIDString();
+    auto transition_mux_task = std::make_unique<UpdateStartAndEndStateTask>("UpdateStartAndEndStateTask",
+                                                                            transition_results.input_key,
+                                                                            prev_output,
+                                                                            next_output,
+                                                                            transition_results.output_key,
+                                                                            false);
     auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task));
 
-    input.data_storage.setData(transition_mux_key, transition_input);
+    input.data_storage.setData(transition_results.input_key, transition_input);
 
     task_graph.addEdges(transition_mux_uuid, { transition_uuid });
     task_graph.addEdges(prev.first, { transition_mux_uuid });
@@ -409,23 +415,24 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input,
   auto from_start_input = program[0].template as<CompositeInstruction>();
   from_start_input.setManipulatorInfo(from_start_input.getManipulatorInfo().getCombined(program_manip_info));
 
-  auto from_start_results = freespace_task_factory_("From Start: " + from_start_input.getDescription(), 1);
+  auto from_start_results = freespace_task_factory_("From Start: " + from_start_input.getDescription(), 0);
   auto from_start_pipeline_uuid = task_graph.addNode(std::move(from_start_results.node));
 
   const auto& first_raster_output_key = raster_tasks[0].second.second;
-  auto update_end_state_task = std::make_unique<UpdateEndStateTask>(
-      "UpdateEndStateTask", first_raster_output_key, from_start_results.input_key, false);
-  std::string update_end_state_key = update_end_state_task->getUUIDString();
+  auto update_end_state_task = std::make_unique<UpdateEndStateTask>("UpdateEndStateTask",
+                                                                    from_start_results.input_key,
+                                                                    first_raster_output_key,
+                                                                    from_start_results.output_key,
+                                                                    false);
   auto update_end_state_uuid = task_graph.addNode(std::move(update_end_state_task));
 
-  input.data_storage.setData(update_end_state_key, from_start_input);
+  input.data_storage.setData(from_start_results.input_key, from_start_input);
 
   task_graph.addEdges(update_end_state_uuid, { from_start_pipeline_uuid });
   task_graph.addEdges(raster_tasks[0].first, { update_end_state_uuid });
 
   // Plan to_end - preceded by the last raster
   auto to_end_input = program.back().template as<CompositeInstruction>();
-
   to_end_input.setManipulatorInfo(to_end_input.getManipulatorInfo().getCombined(program_manip_info));
 
   // Get Start Plan Instruction
@@ -436,16 +443,15 @@ TaskComposerNodeInfo::UPtr RasterMotionTask::runImpl(TaskComposerInput& input,
   assert(li != nullptr);
   to_end_input.insertMoveInstruction(to_end_input.begin(), *li);
 
-  auto to_end_results = freespace_task_factory_("To End: " + to_end_input.getDescription(), 2);
+  auto to_end_results = freespace_task_factory_("To End: " + to_end_input.getDescription(), program.size());
   auto to_end_pipeline_uuid = task_graph.addNode(std::move(to_end_results.node));
 
   const auto& last_raster_output_key = raster_tasks.back().second.second;
   auto update_start_state_task = std::make_unique<UpdateStartStateTask>(
-      "UpdateStartStateTask", last_raster_output_key, to_end_results.input_key, false);
-  std::string update_start_state_key = update_start_state_task->getUUIDString();
+      "UpdateStartStateTask", to_end_results.input_key, last_raster_output_key, to_end_results.output_key, false);
   auto update_start_state_uuid = task_graph.addNode(std::move(update_start_state_task));
 
-  input.data_storage.setData(update_start_state_key, to_end_input);
+  input.data_storage.setData(to_end_results.input_key, to_end_input);
 
   task_graph.addEdges(update_start_state_uuid, { to_end_pipeline_uuid });
   task_graph.addEdges(raster_tasks.back().first, { update_start_state_uuid });
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
@@ -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,19 +338,23 @@ 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));
 
     const auto& prev = raster_tasks[transition_idx];
     const auto& next = raster_tasks[transition_idx + 1];
     const auto& prev_output = prev.second.second;
     const auto& next_output = next.second.second;
-    auto transition_mux_task = std::make_unique<UpdateStartAndEndStateTask>(
-        "UpdateStartAndEndStateTask", prev_output, next_output, transition_results.input_key, false);
-    std::string transition_mux_key = transition_mux_task->getUUIDString();
+    auto transition_mux_task = std::make_unique<UpdateStartAndEndStateTask>("UpdateStartAndEndStateTask",
+                                                                            transition_results.input_key,
+                                                                            prev_output,
+                                                                            next_output,
+                                                                            transition_results.output_key,
+                                                                            false);
     auto transition_mux_uuid = task_graph.addNode(std::move(transition_mux_task));
 
-    input.data_storage.setData(transition_mux_key, transition_input);
+    input.data_storage.setData(transition_results.input_key, transition_input);
 
     task_graph.addEdges(transition_mux_uuid, { transition_uuid });
     task_graph.addEdges(prev.first, { transition_mux_uuid });
diff --git a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp
@@ -139,6 +139,8 @@ TaskflowTaskComposerExecutor::convertToTaskflow(const TaskComposerGraph& task_gr
   // Must add a Node Info object for the graph
   auto info = std::make_unique<TaskComposerNodeInfo>(task_graph);
   info->color = "green";
+  info->input_keys = task_graph.getInputKeys();
+  info->output_keys = task_graph.getOutputKeys();
   task_input.task_infos.addInfo(std::move(info));
 
   // Generate process tasks for each node
diff --git a/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_core_unit.cpp
diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp
