Improve dynamic tasking support

tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp

@@ -130,7 +130,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite,
       if (start_instruction.isNull())
       {
         start_instruction = i.as<MoveInstructionPoly>();
-        composite.push_back(i); // Prevents loss of very first waypoint when upsampling
+        composite.push_back(i);  // Prevents loss of very first waypoint when upsampling
         continue;
       }
 

tesseract_task_composer/taskflow/include/tesseract_task_composer/taskflow/taskflow_task_composer_executor.h

@@ -84,6 +84,10 @@ class TaskflowTaskComposerExecutor : public TaskComposerExecutor
   std::size_t num_threads_;
   std::unique_ptr<tf::Executor> executor_;
 
+  std::mutex futures_mutex_;
+  std::map<boost::uuids::uuid, TaskComposerFuture::UPtr> futures_;
+  void removeFuture(const boost::uuids::uuid& uuid);
+
   TaskComposerFuture::UPtr run(const TaskComposerNode& node, TaskComposerContext::Ptr context) override final;
 
   static tf::Task convertToTaskflow(const TaskComposerGraph& task_graph,

tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp

@@ -30,6 +30,8 @@
 #include <tesseract_common/utils.h>
 #include <tesseract_common/timer.h>
 #include <taskflow/taskflow.hpp>
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
 
 namespace tesseract_planning
 {
@@ -71,6 +73,12 @@ TaskflowTaskComposerExecutor::TaskflowTaskComposerExecutor(std::string name, con
 
 TaskflowTaskComposerExecutor::~TaskflowTaskComposerExecutor() = default;
 
+void TaskflowTaskComposerExecutor::removeFuture(const boost::uuids::uuid& uuid)
+{
+  std::unique_lock<std::mutex> lock(futures_mutex_);
+  futures_.erase(uuid);
+}
+
 TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNode& node,
                                                            TaskComposerContext::Ptr context)
 {
@@ -84,9 +92,14 @@ TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNod
   else
     throw std::runtime_error("TaskComposerExecutor, unsupported node type!");
 
-  std::shared_future<void> f = executor_->run(*taskflow);
-
-  return std::make_unique<TaskflowTaskComposerFuture>(f, std::move(taskflow), std::move(context));
+  // Inorder to better support dynamic tasking within pipelines we store all futures internally
+  // and cleanup when finished because the data cannot go out of scope.
+  std::unique_lock<std::mutex> lock(futures_mutex_);
+  boost::uuids::uuid uuid = boost::uuids::random_generator()();
+  std::shared_future<void> f = executor_->run(*taskflow, [this, uuid]() { removeFuture(uuid); });
+  auto future = std::make_unique<TaskflowTaskComposerFuture>(f, std::move(taskflow), std::move(context));
+  futures_[uuid] = future->copy();
+  return future;
 }
 
 long TaskflowTaskComposerExecutor::getWorkerCount() const { return static_cast<long>(executor_->num_workers()); }