Update taskflows to leverage graph taskflow

tesseract_process_managers/CMakeLists.txt

@@ -59,10 +59,12 @@ add_library(
   src/core/default_process_planners.cpp
   src/core/taskflow_container.cpp
   src/core/utils.cpp
+  src/task_generators/check_input_task_generator.cpp
   src/task_generators/continuous_contact_check_task_generator.cpp
   src/task_generators/discrete_contact_check_task_generator.cpp
   src/task_generators/fix_state_bounds_task_generator.cpp
   src/task_generators/fix_state_collision_task_generator.cpp
+  src/task_generators/has_seed_task_generator.cpp
   src/task_generators/iterative_spline_parameterization_task_generator.cpp
   src/task_generators/motion_planner_task_generator.cpp
   src/task_generators/profile_switch_task_generator.cpp
@@ -75,13 +77,7 @@ add_library(
   src/taskflow_generators/raster_only_global_taskflow.cpp
   src/taskflow_generators/raster_dt_taskflow.cpp
   src/taskflow_generators/raster_waad_taskflow.cpp
-  src/taskflow_generators/raster_waad_dt_taskflow.cpp
-  src/taskflow_generators/descartes_taskflow.cpp
-  src/taskflow_generators/ompl_taskflow.cpp
-  src/taskflow_generators/trajopt_ifopt_taskflow.cpp
-  src/taskflow_generators/trajopt_taskflow.cpp
-  src/taskflow_generators/cartesian_taskflow.cpp
-  src/taskflow_generators/freespace_taskflow.cpp)
+  src/taskflow_generators/raster_waad_dt_taskflow.cpp)
 target_link_libraries(
   ${PROJECT_NAME}
   PUBLIC console_bridge::console_bridge

tesseract_process_managers/examples/taskflow_profiling_example.cpp

@@ -40,9 +40,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/utils/utils.h>
 
 #include <tesseract_process_managers/utils/task_info_statistics.h>
-#include <tesseract_process_managers/taskflow_generators/freespace_taskflow.h>
 #include <tesseract_process_managers/core/process_planning_server.h>
 #include <tesseract_process_managers/utils/task_info_statistics.h>
+#include <tesseract_process_managers/core/default_process_planners.h>
 #include <tesseract_visualization/visualization_loader.h>
 
 using namespace tesseract_planning;
@@ -149,13 +149,12 @@ int main()
   auto display_info = profiler.getTaskDisplayInfo();
 
   /// @todo When FreespaceTaskflow is rewritten to inherit from GraphTaskflow this will work.
-  //  std::ofstream out_data;
-  //  out_data.open(tesseract_common::getTempPath() + "profiling_taskflow-" + tesseract_common::getTimestampString() +
-  //                ".dot");
-  //  FreespaceTaskflowParams params;
-  //  auto graph_taskflow = std::make_unique<FreespaceTaskflow>(params);
-  //  dump(out_data, graph_taskflow, display_info);
-  //  out_data.close();
+  std::ofstream out_data;
+  out_data.open(tesseract_common::getTempPath() + "profiling_taskflow-" + tesseract_common::getTimestampString() +
+                ".dot");
+  auto graph_taskflow = createFreespaceGenerator();
+  dump(out_data, dynamic_cast<const GraphTaskflow&>(*graph_taskflow), display_info);
+  out_data.close();
 
   return 0;
 }

tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h

@@ -31,22 +31,41 @@
 namespace tesseract_planning
 {
 /** @brief Create TrajOpt Process Pipeline */
-TaskflowGenerator::UPtr createTrajOptGenerator();
+TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input = true, bool post_collisin_check = true);
 
 /** @brief Create TrajOpt IFOPT Process Pipeline */
-TaskflowGenerator::UPtr createTrajOptIfoptGenerator();
+TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input = true, bool post_collisin_check = true);
 
 /** @brief Create OMPL Process Pipeline */
-TaskflowGenerator::UPtr createOMPLGenerator();
+TaskflowGenerator::UPtr createOMPLGenerator(bool check_input = true, bool post_collisin_check = true);
 
 /** @brief Create Descartes Process Pipeline */
-TaskflowGenerator::UPtr createDescartesGenerator();
+TaskflowGenerator::UPtr createDescartesGenerator(bool check_input = true, bool post_collisin_check = true);
+
+/**
+ *  @brief Create Descartes Only Process Pipeline
+ *  @details This does not perform post collision check or time parameterization
+ */
+TaskflowGenerator::UPtr createDescartesOnlyGenerator(bool check_input = true);
+
+/**
+ *  @brief Create Descartes No Collision Check Process Pipeline
+ *  @details This does not perform post collision check
+ */
+TaskflowGenerator::UPtr createDescartesNoCollisionGenerator(bool check_input = true);
 
 /** @brief Create Cartesian Process Pipeline */
-TaskflowGenerator::UPtr createCartesianGenerator();
+TaskflowGenerator::UPtr createCartesianGenerator(bool check_input = true);
 
 /** @brief Create Freespace Process Pipeline */
-TaskflowGenerator::UPtr createFreespaceGenerator();
+TaskflowGenerator::UPtr createFreespaceGenerator(bool check_input = true);
+
+/**
+ *  @brief Create Freespace TrajOpt First Process Pipeline
+ *  @details This will try trajopt first before OMPL, but if trajopt fails
+ *  it runs OMPL followed by trajopt.
+ */
+TaskflowGenerator::UPtr createFreespaceTrajOptFirstGenerator(bool check_input = true);
 
 /** @brief Create Raster Process Pipeline */
 TaskflowGenerator::UPtr createRasterGenerator();

tesseract_process_managers/include/tesseract_process_managers/core/task_input.h

@@ -111,7 +111,7 @@ struct TaskInput
   /**
    * @brief This indicates if a seed was provided
    * @details In the case of the raster process planner a skeleton seed is provided which make it
-   * computationaly intensive to determine if a seed was provide so this was added.
+   * computationally intensive to determine if a seed was provide so this was added.
    */
   const bool has_seed{ false };
 
@@ -124,7 +124,7 @@ struct TaskInput
 
   /**
    * @brief Gets the number of instructions contained in the TaskInput
-   * @return 1 instruction if not a composite, otherwise size of the composite @todo Should this be -1, becuase
+   * @return 1 instruction if not a composite, otherwise size of the composite @todo Should this be -1, because
    * composite size could be 1, 0, or other?
    */
   std::size_t size();
@@ -182,19 +182,19 @@ struct TaskInput
   /** @brief Results/Seed for this process */
   Instruction* results_;
 
-  /** @brief The indicies used to access this process inputs instructions and results */
+  /** @brief The indices used to access this process inputs instructions and results */
   std::vector<std::size_t> instruction_indice_;
 
-  /** @brief This proccess inputs start instruction */
+  /** @brief This process inputs start instruction */
   Instruction start_instruction_{ NullInstruction() };
 
-  /** @brief Indices to the start instruction in the results data struction */
+  /** @brief Indices to the start instruction in the results data structure */
   std::vector<std::size_t> start_instruction_indice_;
 
-  /** @brief This proccess inputs end instruction */
+  /** @brief This process inputs end instruction */
   Instruction end_instruction_{ NullInstruction() };
 
-  /** @brief Indices to the end instruction in the results data struction */
+  /** @brief Indices to the end instruction in the results data structure */
   std::vector<std::size_t> end_instruction_indice_;
 
   /** @brief Used to store if process input is aborted which is thread safe */

tesseract_process_managers/include/tesseract_process_managers/task_generators/check_input_task_generator.h

@@ -0,0 +1,79 @@
+/**
+ * @file check_input_task_generator.h
+ * @brief Process generator for checking input data structure
+ *
+ * @author Levi Armstrong
+ * @date November 2. 2021
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2021, Southwest Research Institute
+ *
+ * @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_PROCESS_MANAGERS_CHECK_INPUT_TASK_GENERATOR_H
+#define TESSERACT_PROCESS_MANAGERS_CHECK_INPUT_TASK_GENERATOR_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <functional>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_process_managers/core/task_generator.h>
+
+namespace tesseract_planning
+{
+using CheckInputFn = std::function<bool(const TaskInput&)>;
+
+class CheckInputTaskGenerator : public TaskGenerator
+{
+public:
+  using UPtr = std::unique_ptr<CheckInputTaskGenerator>;
+
+  CheckInputTaskGenerator(std::string name = "Check TaksInput");
+  CheckInputTaskGenerator(CheckInputFn fn, std::string name = "Check TaksInput");
+
+  ~CheckInputTaskGenerator() override = default;
+  CheckInputTaskGenerator(const CheckInputTaskGenerator&) = delete;
+  CheckInputTaskGenerator& operator=(const CheckInputTaskGenerator&) = delete;
+  CheckInputTaskGenerator(CheckInputTaskGenerator&&) = delete;
+  CheckInputTaskGenerator& operator=(CheckInputTaskGenerator&&) = delete;
+
+  int conditionalProcess(TaskInput input, std::size_t unique_id) const override;
+
+  void process(TaskInput input, std::size_t unique_id) const override;
+
+protected:
+  CheckInputFn fn_;
+
+  /**
+   * @brief Checks that the TaskInput is in the correct format.
+   * @param input TaskInput to be checked
+   * @return True if in the correct format
+   */
+  static bool checkTaskInput(const TaskInput& input);
+};
+
+class CheckInputTaskInfo : public TaskInfo
+{
+public:
+  using Ptr = std::shared_ptr<CheckInputTaskInfo>;
+  using ConstPtr = std::shared_ptr<const CheckInputTaskInfo>;
+
+  CheckInputTaskInfo(std::size_t unique_id, std::string name = "Check TaksInput");
+};
+}  // namespace tesseract_planning
+
+#endif  // TESSERACT_PROCESS_MANAGERS_CHECK_INPUT_TASK_GENERATOR_H

tesseract_process_managers/include/tesseract_process_managers/task_generators/has_seed_task_generator.h

@@ -0,0 +1,61 @@
+/**
+ * @file has_seed_task_generator.h
+ * @brief Process generator for checking if the request already has a seed
+ *
+ * @author Levi Armstrong
+ * @date November 2. 2021
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2021, Southwest Research Institute
+ *
+ * @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_PROCESS_MANAGERS_HAS_SEED_TASK_GENERATOR_H
+#define TESSERACT_PROCESS_MANAGERS_HAS_SEED_TASK_GENERATOR_H
+
+#include <tesseract_process_managers/core/task_generator.h>
+
+namespace tesseract_planning
+{
+class HasSeedTaskGenerator : public TaskGenerator
+{
+public:
+  using UPtr = std::unique_ptr<HasSeedTaskGenerator>;
+
+  HasSeedTaskGenerator(std::string name = "Has Seed");
+
+  ~HasSeedTaskGenerator() override = default;
+  HasSeedTaskGenerator(const HasSeedTaskGenerator&) = delete;
+  HasSeedTaskGenerator& operator=(const HasSeedTaskGenerator&) = delete;
+  HasSeedTaskGenerator(HasSeedTaskGenerator&&) = delete;
+  HasSeedTaskGenerator& operator=(HasSeedTaskGenerator&&) = delete;
+
+  int conditionalProcess(TaskInput input, std::size_t unique_id) const override;
+
+  void process(TaskInput input, std::size_t unique_id) const override;
+};
+
+class HasSeedTaskInfo : public TaskInfo
+{
+public:
+  using Ptr = std::shared_ptr<HasSeedTaskInfo>;
+  using ConstPtr = std::shared_ptr<const HasSeedTaskInfo>;
+
+  HasSeedTaskInfo(std::size_t unique_id, std::string name = "Has Seed");
+};
+}  // namespace tesseract_planning
+
+#endif  // TESSERACT_PROCESS_MANAGERS_HAS_SEED_TASK_GENERATOR_H