From 79a37e563952fbfe7fc857a66d0908572991fa74 Mon Sep 17 00:00:00 2001 From: Matthew Powelson Date: Wed, 6 Jan 2021 18:20:18 -0700 Subject: [PATCH] Move ProcessInfo into ProcessInterface for outside access (#514) * Move ProcessInfo into ProcessInterface for outside access * Rename Process to Task for generators and associated types ProcessGenerator -> TaskGenerator ProcessInterface -> TaskflowInterface ProcessInfo -> TaskInfo ProcessInfoContainer -> TaskInfoContainer ProcessInput -> TaskInput * Fix remaining changes Co-authored-by: Levi Armstrong --- .../tesseract_process_managers/CMakeLists.txt | 24 ++-- .../tesseract_process_managers/README.md | 8 +- .../examples/subtaskflow_example.cpp | 4 +- .../core/process_info.h | 87 -------------- .../core/process_planning_future.h | 5 +- .../{process_generator.h => task_generator.h} | 38 +++---- .../core/task_info.h | 88 ++++++++++++++ .../core/{process_input.h => task_input.h} | 107 +++++++++--------- .../core/taskflow_container.h | 6 +- .../core/taskflow_generator.h | 8 +- ...ocess_interface.h => taskflow_interface.h} | 41 +++++-- .../tesseract_process_managers/core/utils.h | 10 +- ...ntinuous_contact_check_process_generator.h | 69 ----------- ...discrete_contact_check_process_generator.h | 72 ------------ .../motion_planner_process_generator.h | 64 ----------- .../continuous_contact_check_task_generator.h | 69 +++++++++++ .../discrete_contact_check_task_generator.h | 72 ++++++++++++ .../fix_state_bounds_task_generator.h} | 34 +++--- .../fix_state_collision_task_generator.h} | 44 +++---- ..._spline_parameterization_task_generator.h} | 44 ++++--- .../motion_planner_task_generator.h | 64 +++++++++++ .../profile_switch_task_generator.h} | 38 +++---- .../seed_min_length_task_generator.h} | 36 +++--- .../taskflow_generators/cartesian_taskflow.h | 8 +- .../taskflow_generators/descartes_taskflow.h | 8 +- .../taskflow_generators/freespace_taskflow.h | 8 +- .../taskflow_generators/graph_taskflow.h | 8 +- .../taskflow_generators/ompl_taskflow.h | 8 +- .../taskflow_generators/raster_dt_taskflow.h | 10 +- .../raster_global_taskflow.h | 12 +- .../raster_only_global_taskflow.h | 12 +- .../raster_only_taskflow.h | 10 +- .../taskflow_generators/raster_taskflow.h | 10 +- .../raster_waad_dt_taskflow.h | 10 +- .../raster_waad_taskflow.h | 10 +- .../taskflow_generators/trajopt_taskflow.h | 8 +- .../src/core/process_planning_server.cpp | 23 ++-- ...ocess_generator.cpp => task_generator.cpp} | 16 +-- .../core/{process_info.cpp => task_info.cpp} | 18 +-- .../{process_input.cpp => task_input.cpp} | 97 ++++++++-------- ...s_interface.cpp => taskflow_interface.cpp} | 26 ++++- .../src/core/utils.cpp | 6 +- ...ntinuous_contact_check_task_generator.cpp} | 32 +++--- ...discrete_contact_check_task_generator.cpp} | 31 +++-- .../fix_state_bounds_task_generator.cpp} | 28 ++--- .../fix_state_collision_task_generator.cpp} | 39 ++++--- ...pline_parameterization_task_generator.cpp} | 24 ++-- .../motion_planner_task_generator.cpp} | 25 ++-- .../profile_switch_task_generator.cpp} | 18 +-- .../seed_min_length_task_generator.cpp} | 34 +++--- .../cartesian_taskflow.cpp | 40 ++++--- .../descartes_taskflow.cpp | 34 +++--- .../freespace_taskflow.cpp | 42 ++++--- .../taskflow_generators/graph_taskflow.cpp | 4 +- .../src/taskflow_generators/ompl_taskflow.cpp | 32 +++--- .../raster_dt_taskflow.cpp | 36 +++--- .../raster_global_taskflow.cpp | 28 ++--- .../raster_only_global_taskflow.cpp | 20 ++-- .../raster_only_taskflow.cpp | 22 ++-- .../taskflow_generators/raster_taskflow.cpp | 30 ++--- .../raster_waad_dt_taskflow.cpp | 54 ++++----- .../raster_waad_taskflow.cpp | 48 ++++---- .../taskflow_generators/trajopt_taskflow.cpp | 36 +++--- .../test/CMakeLists.txt | 18 +-- ...x_state_collision_task_generator_unit.cpp} | 20 ++-- .../test/tesseract_process_managers_unit.cpp | 16 +-- .../swig/tesseract_process_managers_python.i | 16 +-- 67 files changed, 1049 insertions(+), 1018 deletions(-) delete mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_info.h rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/{process_generator.h => task_generator.h} (67%) create mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_info.h rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/{process_input.h => task_input.h} (64%) rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/{process_interface.h => taskflow_interface.h} (59%) delete mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/continuous_contact_check_process_generator.h delete mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/discrete_contact_check_process_generator.h delete mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/motion_planner_process_generator.h create mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/continuous_contact_check_task_generator.h create mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/discrete_contact_check_task_generator.h rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/{process_generators/fix_state_bounds_process_generator.h => task_generators/fix_state_bounds_task_generator.h} (62%) rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/{process_generators/fix_state_collision_process_generator.h => task_generators/fix_state_collision_task_generator.h} (73%) rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/{process_generators/iterative_spline_parameterization_process_generator.h => task_generators/iterative_spline_parameterization_task_generator.h} (58%) create mode 100644 tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/motion_planner_task_generator.h rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/{process_generators/profile_switch_process_generator.h => task_generators/profile_switch_task_generator.h} (54%) rename tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/{process_generators/seed_min_length_process_generator.h => task_generators/seed_min_length_task_generator.h} (53%) rename tesseract/tesseract_planning/tesseract_process_managers/src/core/{process_generator.cpp => task_generator.cpp} (72%) rename tesseract/tesseract_planning/tesseract_process_managers/src/core/{process_info.cpp => task_info.cpp} (64%) rename tesseract/tesseract_planning/tesseract_process_managers/src/core/{process_input.cpp => task_input.cpp} (63%) rename tesseract/tesseract_planning/tesseract_process_managers/src/core/{process_interface.cpp => taskflow_interface.cpp} (55%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/continuous_contact_check_process_generator.cpp => task_generators/continuous_contact_check_task_generator.cpp} (77%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/discrete_contact_check_process_generator.cpp => task_generators/discrete_contact_check_task_generator.cpp} (78%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/fix_state_bounds_process_generator.cpp => task_generators/fix_state_bounds_task_generator.cpp} (81%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/fix_state_collision_process_generator.cpp => task_generators/fix_state_collision_task_generator.cpp} (90%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/iterative_spline_parameterization_process_generator.cpp => task_generators/iterative_spline_parameterization_task_generator.cpp} (85%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/motion_planner_process_generator.cpp => task_generators/motion_planner_task_generator.cpp} (85%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/profile_switch_process_generator.cpp => task_generators/profile_switch_task_generator.cpp} (78%) rename tesseract/tesseract_planning/tesseract_process_managers/src/{process_generators/seed_min_length_process_generator.cpp => task_generators/seed_min_length_task_generator.cpp} (75%) rename tesseract/tesseract_planning/tesseract_process_managers/test/{fix_state_collision_process_generator_unit.cpp => fix_state_collision_task_generator_unit.cpp} (89%) diff --git a/tesseract/tesseract_planning/tesseract_process_managers/CMakeLists.txt b/tesseract/tesseract_planning/tesseract_process_managers/CMakeLists.txt index 5992ef8f4c6..fd3f0a40ad6 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/CMakeLists.txt +++ b/tesseract/tesseract_planning/tesseract_process_managers/CMakeLists.txt @@ -41,25 +41,25 @@ include(GenerateExportHeader) # Create interface add_library(${PROJECT_NAME} - src/core/process_input.cpp + src/core/task_input.cpp src/core/debug_observer.cpp - src/core/process_generator.cpp + src/core/task_generator.cpp src/core/process_planning_future.cpp src/core/process_planning_server.cpp src/core/process_environment_cache.cpp - src/core/process_interface.cpp - src/core/process_info.cpp + src/core/taskflow_interface.cpp + src/core/task_info.cpp src/core/default_process_planners.cpp src/core/taskflow_container.cpp src/core/utils.cpp - src/process_generators/continuous_contact_check_process_generator.cpp - src/process_generators/discrete_contact_check_process_generator.cpp - src/process_generators/fix_state_bounds_process_generator.cpp - src/process_generators/fix_state_collision_process_generator.cpp - src/process_generators/iterative_spline_parameterization_process_generator.cpp - src/process_generators/motion_planner_process_generator.cpp - src/process_generators/profile_switch_process_generator.cpp - src/process_generators/seed_min_length_process_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/iterative_spline_parameterization_task_generator.cpp + src/task_generators/motion_planner_task_generator.cpp + src/task_generators/profile_switch_task_generator.cpp + src/task_generators/seed_min_length_task_generator.cpp src/taskflow_generators/graph_taskflow.cpp src/taskflow_generators/raster_taskflow.cpp src/taskflow_generators/raster_global_taskflow.cpp diff --git a/tesseract/tesseract_planning/tesseract_process_managers/README.md b/tesseract/tesseract_planning/tesseract_process_managers/README.md index a20d7b1e13f..57cb5eb7975 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/README.md +++ b/tesseract/tesseract_planning/tesseract_process_managers/README.md @@ -6,12 +6,12 @@ This package contains process managers for Tesseract. Examples include * Raster strip planning manager ## Overview -### ProcessInput -This package uses [Taskflow](https://github.com/taskflow/taskflow) to organize and perform robotics tasks. Each of the tasks in the system will take a ProcessInput which is just a Tesseract::ConstPtr and references to instructions and the seed. +### TaskInput +This package uses [Taskflow](https://github.com/taskflow/taskflow) to organize and perform robotics tasks. Each of the tasks in the system will take a TaskInput which is just a Tesseract::ConstPtr and references to instructions and the seed. -Each Task will operate on the instructions and the results will get stored in the seed. The ProcessInput does not own the instructions in order to keep it lightweight and able to be segmented into sub-ProcessInputs (see [] operator). Therefore, the instructions and seed passed into the ProcessInput must be kept alive by the user. +Each Task will operate on the instructions and the results will get stored in the seed. The TaskInput does not own the instructions in order to keep it lightweight and able to be segmented into sub-TaskInputs (see [] operator). Therefore, the instructions and seed passed into the TaskInput must be kept alive by the user. -It is also worth noting that ProcessInput is not inherently thread-safe. Since taskflow will attempt to execute as many tasks in parallel as possible, it is the responsibility of the user to ensure that Tasks that use the same portions of the ProcessInput are not running at the same time. In practice, this shouldn't be difficult as many of the planning operations currently implemented have a clear order. However, you should not, for example, try to plan the same segment using two different planners at the same time without first making a copy of the inputs to the ProcessInput for each Task. +It is also worth noting that TaskInput is not inherently thread-safe. Since taskflow will attempt to execute as many tasks in parallel as possible, it is the responsibility of the user to ensure that Tasks that use the same portions of the TaskInput are not running at the same time. In practice, this shouldn't be difficult as many of the planning operations currently implemented have a clear order. However, you should not, for example, try to plan the same segment using two different planners at the same time without first making a copy of the inputs to the TaskInput for each Task. ### Class Structure The package is divided into several types of classes diff --git a/tesseract/tesseract_planning/tesseract_process_managers/examples/subtaskflow_example.cpp b/tesseract/tesseract_planning/tesseract_process_managers/examples/subtaskflow_example.cpp index 9ea6964a703..defb65518bf 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/examples/subtaskflow_example.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/examples/subtaskflow_example.cpp @@ -1,7 +1,7 @@ #include #include -struct ProcessInput +struct TaskInput { int* cnt; }; @@ -18,7 +18,7 @@ int main(int /*argc*/, char** /*argv*/) tf::Taskflow taskflow; int num_subtaskflows = 1; - ProcessInput input; + TaskInput input; input.cnt = &num_subtaskflows; tf::Task t = taskflow.placeholder(); diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_info.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_info.h deleted file mode 100644 index 612e90083ff..00000000000 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_info.h +++ /dev/null @@ -1,87 +0,0 @@ -/** - * @file process_info.h - * @brief Process Info - * - * @author Matthew Powelson - * @date July 15. 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, 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_PROCESS_INFO_H -#define TESSERACT_PROCESS_MANAGERS_PROCESS_INFO_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#ifdef SWIG -%shared_ptr(tesseract_planning::ProcessInfo) -#endif // SWIG - -namespace tesseract_planning -{ -class ProcessInfo -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - ProcessInfo(std::size_t unique_id, std::string name = ""); - virtual ~ProcessInfo() = default; - ProcessInfo(const ProcessInfo&) = default; - ProcessInfo& operator=(const ProcessInfo&) = default; - ProcessInfo(ProcessInfo&&) = default; - ProcessInfo& operator=(ProcessInfo&&) = default; - - int return_value; - - std::size_t unique_id; - - std::string process_name; - - std::string message; -}; -} // namespace tesseract_planning - -#ifdef SWIG -%template(ProcessInfoMap) std::map>; -#endif - -namespace tesseract_planning -{ -/** @brief A threadsafe container for ProcessInfos */ -struct ProcessInfoContainer -{ - void addProcessInfo(ProcessInfo::ConstPtr process_info); - - ProcessInfo::ConstPtr operator[](std::size_t index) const; - - /** @brief Get a copy of the process_info_vec in case it gets resized*/ - std::map getProcessInfoMap() const; - -private: - mutable std::shared_mutex mutex_; - std::map process_info_map_; -}; -} // namespace tesseract_planning - -#endif // TESSERACT_PROCESS_MANAGERS_PROCESS_INFO_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_future.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_future.h index 96958eda590..d9fd4711c9e 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_future.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_future.h @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include @@ -60,7 +60,7 @@ struct ProcessPlanningFuture std::future process_future; /** @brief This is used to abort the associated process and check if the process was successful */ - ProcessInterface::Ptr interface; + TaskflowInterface::Ptr interface; #ifndef SWIG /** @brief The stored input to the process */ @@ -77,6 +77,7 @@ struct ProcessPlanningFuture /** @brief The stored composite profile remapping */ std::unique_ptr composite_profile_remapping; + #else // clang-format off %extend { diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_generator.h similarity index 67% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_generator.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_generator.h index 6f4e5922495..2c3a7169c7e 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_generator.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_generator.h @@ -1,5 +1,5 @@ /** - * @file process_generator.h + * @file task_generator.h * @brief Process generator * * @author Matthew Powelson @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_PROCESS_GENERATOR_H +#ifndef TESSERACT_PROCESS_MANAGERS_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_TASK_GENERATOR_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH @@ -33,28 +33,28 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_planning { /** * @brief This is a base class for generating instances of processes as tasks such that they may be executed in - * parallel. A typical workflow would be task t = process_generator.generateTask(input, taskflow) + * parallel. A typical workflow would be task t = task_generator.generateTask(input, taskflow) * * Only unique pointers should be used because of the ability to abort the process. With recent changes this may no * longer be valid but need to investigate. */ -class ProcessGenerator +class TaskGenerator { public: - using UPtr = std::unique_ptr; + using UPtr = std::unique_ptr; - ProcessGenerator(std::string name = ""); - virtual ~ProcessGenerator() = default; - ProcessGenerator(const ProcessGenerator&) = delete; - ProcessGenerator& operator=(const ProcessGenerator&) = delete; - ProcessGenerator(ProcessGenerator&&) = delete; - ProcessGenerator& operator=(ProcessGenerator&&) = delete; + TaskGenerator(std::string name = ""); + virtual ~TaskGenerator() = default; + TaskGenerator(const TaskGenerator&) = delete; + TaskGenerator& operator=(const TaskGenerator&) = delete; + TaskGenerator(TaskGenerator&&) = delete; + TaskGenerator& operator=(TaskGenerator&&) = delete; /** * @brief Get the task name @@ -68,14 +68,14 @@ class ProcessGenerator * @param taskflow The taskflow to associate the task with * @return Task */ - virtual tf::Task generateTask(ProcessInput input, tf::Taskflow& taskflow); + virtual tf::Task generateTask(TaskInput input, tf::Taskflow& taskflow); /** * @brief Assign work to the provided task * @param input The process input * @param task The task to assign the work to */ - virtual void assignTask(ProcessInput input, tf::Task& task); + virtual void assignTask(TaskInput input, tf::Task& task); /** * @brief Generated a Task @@ -83,14 +83,14 @@ class ProcessGenerator * @param taskflow The taskflow to associate the task with * @return Conditional Task */ - virtual tf::Task generateConditionalTask(ProcessInput input, tf::Taskflow& taskflow); + virtual tf::Task generateConditionalTask(TaskInput input, tf::Taskflow& taskflow); /** * @brief Assign work to the provided task * @param input The process input * @param task The task to assign the work to */ - virtual void assignConditionalTask(ProcessInput input, tf::Task& task); + virtual void assignConditionalTask(TaskInput input, tf::Task& task); protected: /** @brief The name of the process */ @@ -101,14 +101,14 @@ class ProcessGenerator * @param input The process input * @return Task */ - virtual void process(ProcessInput input, std::size_t unique_id) const = 0; + virtual void process(TaskInput input, std::size_t unique_id) const = 0; /** * @brief Generate Conditional Task * @param input The process input * @return Conditional Task */ - virtual int conditionalProcess(ProcessInput input, std::size_t unique_id) const = 0; + virtual int conditionalProcess(TaskInput input, std::size_t unique_id) const = 0; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_info.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_info.h new file mode 100644 index 00000000000..b2105b0f0d4 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_info.h @@ -0,0 +1,88 @@ +/** + * @file task_info.h + * @brief Process Info + * + * @author Matthew Powelson + * @date July 15. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, 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_task_info_H +#define TESSERACT_PROCESS_MANAGERS_task_info_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#ifdef SWIG +%shared_ptr(tesseract_planning::TaskInfo) +%template(TaskInfoMap) std::map>; +%shared_ptr(tesseract_planning::TaskInfoContainer) +#endif // SWIG + +namespace tesseract_planning +{ +/** Stores information about a Task */ +class TaskInfo +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + TaskInfo(std::size_t unique_id, std::string name = ""); + virtual ~TaskInfo() = default; + TaskInfo(const TaskInfo&) = default; + TaskInfo& operator=(const TaskInfo&) = default; + TaskInfo(TaskInfo&&) = default; + TaskInfo& operator=(TaskInfo&&) = default; + + /** @brief Value returned from the Task on completion */ + int return_value; + + /** @brief Unique ID generated for the Task by Taskflow */ + std::size_t unique_id; + + std::string task_name; + + std::string message; +}; + +/** @brief A threadsafe container for TaskInfos */ +struct TaskInfoContainer +{ + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + void addTaskInfo(TaskInfo::ConstPtr task_info); + + TaskInfo::ConstPtr operator[](std::size_t index) const; + + /** @brief Get a copy of the task_info_map_ in case it gets resized*/ + std::map getTaskInfoMap() const; + +private: + mutable std::shared_mutex mutex_; + std::map task_info_map_; +}; +} // namespace tesseract_planning + +#endif // TESSERACT_PROCESS_MANAGERS_task_info_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_input.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_input.h similarity index 64% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_input.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_input.h index 87069516624..9d3bf66d145 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_input.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/task_input.h @@ -1,5 +1,5 @@ -/** - * @file process_input.h +/** + * @file task_input.h * @brief Process input * * @author Matthew Powelson @@ -23,8 +23,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_PROCESS_INPUT_H -#define TESSERACT_PROCESS_MANAGERS_PROCESS_INPUT_H +#ifndef TESSERACT_PROCESS_MANAGERS_task_input_H +#define TESSERACT_PROCESS_MANAGERS_task_input_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH @@ -33,8 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include +#include #include #include @@ -49,43 +49,43 @@ namespace tesseract_planning /** * @brief This struct is passed as an input to each process in the decision tree * - * Note that it does not have ownership of any of its members (except the pointer). This means that if a ProcessInput + * Note that it does not have ownership of any of its members (except the pointer). This means that if a TaskInput * spawns a child that is a subset, it does not have to remain in scope as the references will still be valid */ -struct ProcessInput +struct TaskInput { - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - const ManipulatorInfo& manip_info, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles); - - ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - const ManipulatorInfo& manip_info, - const PlannerProfileRemapping& plan_profile_remapping, - const PlannerProfileRemapping& composite_profile_remapping, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles); - - ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - const PlannerProfileRemapping& plan_profile_remapping, - const PlannerProfileRemapping& composite_profile_remapping, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles); - - ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles); + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + const ManipulatorInfo& manip_info, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles); + + TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + const ManipulatorInfo& manip_info, + const PlannerProfileRemapping& plan_profile_remapping, + const PlannerProfileRemapping& composite_profile_remapping, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles); + + TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + const PlannerProfileRemapping& plan_profile_remapping, + const PlannerProfileRemapping& composite_profile_remapping, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles); + + TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles); /** @brief Tesseract associated with current state of the system */ const tesseract_environment::Environment::ConstPtr env; @@ -116,14 +116,14 @@ struct ProcessInput const bool has_seed{ false }; /** - * @brief Creates a sub-ProcessInput from instruction[index] and seed[index] - * @param index sub-Instruction used to create the ProcessInput - * @return A ProcessInput containing a subset of the original's instructions + * @brief Creates a sub-TaskInput from instruction[index] and seed[index] + * @param index sub-Instruction used to create the TaskInput + * @return A TaskInput containing a subset of the original's instructions */ - ProcessInput operator[](std::size_t index); + TaskInput operator[](std::size_t index); /** - * @brief Gets the number of instructions contained in the ProcessInput + * @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 * composite size could be 1, 0, or other? */ @@ -141,8 +141,11 @@ struct ProcessInput */ Instruction* getResults(); - /** @brief The process interface for checking success and aborting active process */ - ProcessInterface::Ptr getProcessInterface(); + /** + * @brief Gets the task interface for checking success and aborting active process + * @return The task interface for checking success and aborting active process + */ + TaskflowInterface::Ptr getTaskInterface(); /** * @brief Check if process has been aborted @@ -165,9 +168,9 @@ struct ProcessInput void setEndInstruction(std::vector end); Instruction getEndInstruction() const; - void addProcessInfo(const ProcessInfo::ConstPtr& process_info); - ProcessInfo::ConstPtr getProcessInfo(const std::size_t& index) const; - std::map getProcessInfoMap() const; + void addTaskInfo(const TaskInfo::ConstPtr& task_info); + TaskInfo::ConstPtr getTaskInfo(const std::size_t& index) const; + std::map getTaskInfoMap() const; protected: /** @brief Instructions to be carried out by process */ @@ -191,10 +194,8 @@ struct ProcessInput /** @brief Indices to the end instruction in the results data struction */ std::vector end_instruction_indice_; - std::shared_ptr process_infos_{ std::make_shared() }; - /** @brief Used to store if process input is aborted which is thread safe */ - ProcessInterface::Ptr interface_{ std::make_shared() }; + TaskflowInterface::Ptr interface_{ std::make_shared() }; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_container.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_container.h index 4a900028097..f5216b270bd 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_container.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_container.h @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_planning { @@ -78,10 +78,10 @@ struct TaskflowContainer std::vector containers; /** - * @brief ProcessGenerator's associated with the taskflow + * @brief TaskGenerator's associated with the taskflow * @details This must stay around during execution of taskflow */ - std::vector generators; + std::vector generators; /** @brief Clear the Taskflow Container */ void clear(); diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_generator.h index 56040d8c41c..d636f034970 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_generator.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_generator.h @@ -27,8 +27,8 @@ #define TESSERACT_PROCESS_MANAGERS_TASKFLOW_GENERATOR_H #include -#include -#include +#include +#include #include namespace tesseract_planning @@ -59,7 +59,7 @@ class TaskflowGenerator * @param error_cb A user defined callback * @return A taskflow container which must stay around during execution of taskflow */ - virtual TaskflowContainer generateTaskflow(ProcessInput instruction, + virtual TaskflowContainer generateTaskflow(TaskInput instruction, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) = 0; @@ -74,7 +74,7 @@ class TaskflowGenerator // * @return A taskflow container which must stay around during execution of taskflow // */ // virtual TaskflowContainer generateTaskflow(tf::Taskflow& taskflow, - // ProcessInput instruction, + // TaskInput instruction, // TaskflowVoidFn done_cb, // TaskflowVoidFn error_cb) = 0; }; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_interface.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_interface.h similarity index 59% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_interface.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_interface.h index c055edcc018..6b86936e4dc 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/process_interface.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/taskflow_interface.h @@ -1,5 +1,5 @@ /** - * @file process_interface.h + * @file taskflow_interface.h * @brief Process Inteface * * @author Levi Armstrong @@ -23,17 +23,20 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_PROCESS_INTERFACE_H -#define TESSERACT_PROCESS_MANAGERS_PROCESS_INTERFACE_H +#ifndef TESSERACT_PROCESS_MANAGERS_taskflow_interface_H +#define TESSERACT_PROCESS_MANAGERS_taskflow_interface_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + #ifdef SWIG -%shared_ptr(tesseract_planning::ProcessInterface) +%shared_ptr(tesseract_planning::TaskflowInterface) #endif // SWIG namespace tesseract_planning @@ -42,10 +45,10 @@ namespace tesseract_planning * @brief This is a thread safe class used for aborting a process along with checking if a process was succesful * @details If a process failed then the process has been abort by some child process */ -class ProcessInterface +class TaskflowInterface { public: - using Ptr = std::shared_ptr; + using Ptr = std::shared_ptr; /** * @brief Check if the process was aborted @@ -62,10 +65,32 @@ class ProcessInterface /** @brief Abort the process associated with this interface */ void abort(); + /** + * @brief Get TaskInfo for a specific task by unique ID + * @param index Unique ID assigned the task from taskflow + * @return The TaskInfo associated with this task + */ + TaskInfo::ConstPtr getTaskInfo(const std::size_t& index) const; + + /** + * @brief Get the entire stored map of TaskInfos + * @return The map of TaskInfos stored by unique ID + */ + std::map getTaskInfoMap() const; + + /** + * @brief Not meant to be used by users. Exposes TaskInfoContainer so that the + * @return Threadsafe TaskInfo container + */ + TaskInfoContainer::Ptr getTaskInfoContainer() const; + protected: std::atomic abort_{ false }; + + /** @brief Threadsafe container for TaskInfos */ + TaskInfoContainer::Ptr task_infos_{ std::make_shared() }; }; } // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_PROCESS_INTERFACE_H +#endif // TESSERACT_PROCESS_MANAGERS_taskflow_interface_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/utils.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/utils.h index 04b46045b0b..5bdc90ea8f8 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/utils.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/core/utils.h @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include namespace tesseract_planning { @@ -45,19 +45,19 @@ namespace tesseract_planning * @param message A detailed message * @param user_callback A user callback function */ -void successTask(const ProcessInput& /*instruction*/, +void successTask(const TaskInput& /*instruction*/, const std::string& name, const std::string& message, const TaskflowVoidFn& user_callback = nullptr); /** * @brief The default failure task to be used - * @details This will call the abort function of the ProcessInput provided + * @details This will call the abort function of the TaskInput provided * @param name The name * @param message A detailed message * @param user_callback A user callback function */ -void failureTask(ProcessInput instruction, +void failureTask(TaskInput instruction, const std::string& name, const std::string& message, const TaskflowVoidFn& user_callback = nullptr); @@ -75,7 +75,7 @@ bool isCompositeEmpty(const CompositeInstruction& composite); * @param input The process input * @return One if seed exists, otherwise zero */ -int hasSeedTask(ProcessInput input); +int hasSeedTask(TaskInput input); } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/continuous_contact_check_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/continuous_contact_check_process_generator.h deleted file mode 100644 index 728a6c0472c..00000000000 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/continuous_contact_check_process_generator.h +++ /dev/null @@ -1,69 +0,0 @@ -/** - * @file continuous_contact_check_process_generator.h - * @brief Continuous Collision check trajectory - * - * @author Levi Armstrong - * @date August 10. 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, 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_CONTINUOUS_CONTACT_CHECK_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_CONTINUOUS_CONTACT_CHECK_PROCESS_GENERATOR_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tesseract_planning -{ -class ContinuousContactCheckProcessGenerator : public ProcessGenerator -{ -public: - using UPtr = std::unique_ptr; - - ContinuousContactCheckProcessGenerator(std::string name = "Continuous Contact Check Trajectory"); - - ContinuousContactCheckProcessGenerator(double longest_valid_segment_length, - double contact_distance, - std::string name = "Continuous Contact Check Trajectory"); - - ~ContinuousContactCheckProcessGenerator() override = default; - ContinuousContactCheckProcessGenerator(const ContinuousContactCheckProcessGenerator&) = delete; - ContinuousContactCheckProcessGenerator& operator=(const ContinuousContactCheckProcessGenerator&) = delete; - ContinuousContactCheckProcessGenerator(ContinuousContactCheckProcessGenerator&&) = delete; - ContinuousContactCheckProcessGenerator& operator=(ContinuousContactCheckProcessGenerator&&) = delete; - - tesseract_collision::CollisionCheckConfig config; - - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; - - void process(ProcessInput input, std::size_t unique_id) const override; -}; - -class ContinuousContactCheckProcessInfo : public ProcessInfo -{ -public: - ContinuousContactCheckProcessInfo(std::size_t unique_id, std::string name = "Continuous Contact Check Trajectory"); - - std::vector contact_results; -}; -} // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_CONTINUOUS_CONTACT_CHECK_PROCESS_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/discrete_contact_check_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/discrete_contact_check_process_generator.h deleted file mode 100644 index db104f0e381..00000000000 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/discrete_contact_check_process_generator.h +++ /dev/null @@ -1,72 +0,0 @@ -/** - * @file discrete_contact_check_process_generator.h - * @brief Discrete Collision check trajectory - * - * @author Levi Armstrong - * @date August 10. 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, 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_DISCRETE_CONTACT_CHECK_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_DISCRETE_CONTACT_CHECK_PROCESS_GENERATOR_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -class DiscreteContactCheckProcessGenerator : public ProcessGenerator -{ -public: - using UPtr = std::unique_ptr; - - DiscreteContactCheckProcessGenerator(std::string name = "Discrete Contact Check Trajectory"); - - DiscreteContactCheckProcessGenerator(double longest_valid_segment_length, - double contact_distance, - std::string name = "Discrete Contact Check Trajectory"); - - ~DiscreteContactCheckProcessGenerator() override = default; - DiscreteContactCheckProcessGenerator(const DiscreteContactCheckProcessGenerator&) = delete; - DiscreteContactCheckProcessGenerator& operator=(const DiscreteContactCheckProcessGenerator&) = delete; - DiscreteContactCheckProcessGenerator(DiscreteContactCheckProcessGenerator&&) = delete; - DiscreteContactCheckProcessGenerator& operator=(DiscreteContactCheckProcessGenerator&&) = delete; - - tesseract_collision::CollisionCheckConfig config; - - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; - - void process(ProcessInput input, std::size_t unique_id) const override; -}; - -class DiscreteContactCheckProcessInfo : public ProcessInfo -{ -public: - DiscreteContactCheckProcessInfo(std::size_t unique_id, std::string name = "Discrete Contact Check Trajectory"); - - std::vector contact_results; -}; - -} // namespace tesseract_planning - -#endif // TESSERACT_PROCESS_MANAGERS_DISCRETE_CONTACT_CHECK_PROCESS_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/motion_planner_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/motion_planner_process_generator.h deleted file mode 100644 index 6f7ffa75aa4..00000000000 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/motion_planner_process_generator.h +++ /dev/null @@ -1,64 +0,0 @@ -/** - * @file motion_planner_process_generator.h - * @brief Generates a motion planning process - * - * @author Matthew Powelson - * @date July 15. 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, 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_MOTION_PLANNER_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_MOTION_PLANNER_PROCESS_GENERATOR_H - -#include - -namespace tesseract_planning -{ -// Forward Declare -class MotionPlanner; - -class MotionPlannerProcessGenerator : public ProcessGenerator -{ -public: - using UPtr = std::unique_ptr; - - MotionPlannerProcessGenerator(std::shared_ptr planner); - ~MotionPlannerProcessGenerator() override = default; - MotionPlannerProcessGenerator(const MotionPlannerProcessGenerator&) = delete; - MotionPlannerProcessGenerator& operator=(const MotionPlannerProcessGenerator&) = delete; - MotionPlannerProcessGenerator(MotionPlannerProcessGenerator&&) = delete; - MotionPlannerProcessGenerator& operator=(MotionPlannerProcessGenerator&&) = delete; - - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; - - void process(ProcessInput input, std::size_t unique_id) const override; - -private: - std::shared_ptr planner_{ nullptr }; -}; - -class MotionPlannerProcessInfo : public ProcessInfo -{ -public: - MotionPlannerProcessInfo(std::size_t unique_id, std::string name = "Motion Planner Process Generator"); -}; - -} // namespace tesseract_planning - -#endif diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/continuous_contact_check_task_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/continuous_contact_check_task_generator.h new file mode 100644 index 00000000000..1ca90160851 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/continuous_contact_check_task_generator.h @@ -0,0 +1,69 @@ +/** + * @file continuous_contact_check_task_generator.h + * @brief Continuous Collision check trajectory + * + * @author Levi Armstrong + * @date August 10. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, 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_CONTINUOUS_CONTACT_CHECK_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_CONTINUOUS_CONTACT_CHECK_TASK_GENERATOR_H +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class ContinuousContactCheckTaskGenerator : public TaskGenerator +{ +public: + using UPtr = std::unique_ptr; + + ContinuousContactCheckTaskGenerator(std::string name = "Continuous Contact Check Trajectory"); + + ContinuousContactCheckTaskGenerator(double longest_valid_segment_length, + double contact_distance, + std::string name = "Continuous Contact Check Trajectory"); + + ~ContinuousContactCheckTaskGenerator() override = default; + ContinuousContactCheckTaskGenerator(const ContinuousContactCheckTaskGenerator&) = delete; + ContinuousContactCheckTaskGenerator& operator=(const ContinuousContactCheckTaskGenerator&) = delete; + ContinuousContactCheckTaskGenerator(ContinuousContactCheckTaskGenerator&&) = delete; + ContinuousContactCheckTaskGenerator& operator=(ContinuousContactCheckTaskGenerator&&) = delete; + + tesseract_collision::CollisionCheckConfig config; + + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; + + void process(TaskInput input, std::size_t unique_id) const override; +}; + +class ContinuousContactCheckTaskInfo : public TaskInfo +{ +public: + ContinuousContactCheckTaskInfo(std::size_t unique_id, std::string name = "Continuous Contact Check Trajectory"); + + std::vector contact_results; +}; +} // namespace tesseract_planning +#endif // TESSERACT_PROCESS_MANAGERS_CONTINUOUS_CONTACT_CHECK_TASK_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/discrete_contact_check_task_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/discrete_contact_check_task_generator.h new file mode 100644 index 00000000000..fb08bcf7e50 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/discrete_contact_check_task_generator.h @@ -0,0 +1,72 @@ +/** + * @file discrete_contact_check_task_generator.h + * @brief Discrete Collision check trajectory + * + * @author Levi Armstrong + * @date August 10. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, 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_DISCRETE_CONTACT_CHECK_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_DISCRETE_CONTACT_CHECK_TASK_GENERATOR_H +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning +{ +class DiscreteContactCheckTaskGenerator : public TaskGenerator +{ +public: + using UPtr = std::unique_ptr; + + DiscreteContactCheckTaskGenerator(std::string name = "Discrete Contact Check Trajectory"); + + DiscreteContactCheckTaskGenerator(double longest_valid_segment_length, + double contact_distance, + std::string name = "Discrete Contact Check Trajectory"); + + ~DiscreteContactCheckTaskGenerator() override = default; + DiscreteContactCheckTaskGenerator(const DiscreteContactCheckTaskGenerator&) = delete; + DiscreteContactCheckTaskGenerator& operator=(const DiscreteContactCheckTaskGenerator&) = delete; + DiscreteContactCheckTaskGenerator(DiscreteContactCheckTaskGenerator&&) = delete; + DiscreteContactCheckTaskGenerator& operator=(DiscreteContactCheckTaskGenerator&&) = delete; + + tesseract_collision::CollisionCheckConfig config; + + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; + + void process(TaskInput input, std::size_t unique_id) const override; +}; + +class DiscreteContactCheckTaskInfo : public TaskInfo +{ +public: + DiscreteContactCheckTaskInfo(std::size_t unique_id, std::string name = "Discrete Contact Check Trajectory"); + + std::vector contact_results; +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_PROCESS_MANAGERS_DISCRETE_CONTACT_CHECK_TASK_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/fix_state_bounds_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/fix_state_bounds_task_generator.h similarity index 62% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/fix_state_bounds_process_generator.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/fix_state_bounds_task_generator.h index 1ca6cd5d656..f9855cb56c8 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/fix_state_bounds_process_generator.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/fix_state_bounds_task_generator.h @@ -1,5 +1,5 @@ /** - * @file fix_state_bounds_process_generator.h + * @file fix_state_bounds_task_generator.h * @brief Process generator for process that pushes plan instructions back within joint limits * * @author Matthew Powelson @@ -23,10 +23,10 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_PROCESS_GENERATOR_H +#ifndef TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_TASK_GENERATOR_H -#include +#include #include namespace tesseract_planning @@ -58,32 +58,32 @@ using FixStateBoundsProfileMap = std::unordered_map; + using UPtr = std::unique_ptr; - FixStateBoundsProcessGenerator(std::string name = "Fix State Bounds"); + FixStateBoundsTaskGenerator(std::string name = "Fix State Bounds"); - ~FixStateBoundsProcessGenerator() override = default; - FixStateBoundsProcessGenerator(const FixStateBoundsProcessGenerator&) = delete; - FixStateBoundsProcessGenerator& operator=(const FixStateBoundsProcessGenerator&) = delete; - FixStateBoundsProcessGenerator(FixStateBoundsProcessGenerator&&) = delete; - FixStateBoundsProcessGenerator& operator=(FixStateBoundsProcessGenerator&&) = delete; + ~FixStateBoundsTaskGenerator() override = default; + FixStateBoundsTaskGenerator(const FixStateBoundsTaskGenerator&) = delete; + FixStateBoundsTaskGenerator& operator=(const FixStateBoundsTaskGenerator&) = delete; + FixStateBoundsTaskGenerator(FixStateBoundsTaskGenerator&&) = delete; + FixStateBoundsTaskGenerator& operator=(FixStateBoundsTaskGenerator&&) = delete; FixStateBoundsProfileMap composite_profiles; - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; - void process(ProcessInput input, std::size_t unique_id) const override; + void process(TaskInput input, std::size_t unique_id) const override; }; -class FixStateBoundsProcessInfo : public ProcessInfo +class FixStateBoundsTaskInfo : public TaskInfo { public: - FixStateBoundsProcessInfo(std::size_t unique_id, std::string name = "Fix State Bounds"); + FixStateBoundsTaskInfo(std::size_t unique_id, std::string name = "Fix State Bounds"); std::vector contact_results; }; } // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_PROCESS_GENERATOR_H +#endif // TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_TASK_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/fix_state_collision_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/fix_state_collision_task_generator.h similarity index 73% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/fix_state_collision_process_generator.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/fix_state_collision_task_generator.h index 4e9a0bc3dec..de3d4d1b8d1 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/fix_state_collision_process_generator.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/fix_state_collision_task_generator.h @@ -1,5 +1,5 @@ /** - * @file fix_state_collision_process_generator.h + * @file fix_state_collision_task_generator.h * @brief Process generator for process that pushes plan instructions to be out of collision * * @author Matthew Powelson @@ -23,10 +23,10 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_FIX_STATE_COLLISION_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_FIX_STATE_COLLISION_PROCESS_GENERATOR_H +#ifndef TESSERACT_PROCESS_MANAGERS_FIX_STATE_COLLISION_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_FIX_STATE_COLLISION_TASK_GENERATOR_H -#include +#include namespace tesseract_planning { @@ -77,30 +77,30 @@ using FixStateCollisionProfileMap = std::unordered_map; + using UPtr = std::unique_ptr; - FixStateCollisionProcessGenerator(std::string name = "Fix State Collision"); + FixStateCollisionTaskGenerator(std::string name = "Fix State Collision"); - ~FixStateCollisionProcessGenerator() override = default; - FixStateCollisionProcessGenerator(const FixStateCollisionProcessGenerator&) = delete; - FixStateCollisionProcessGenerator& operator=(const FixStateCollisionProcessGenerator&) = delete; - FixStateCollisionProcessGenerator(FixStateCollisionProcessGenerator&&) = delete; - FixStateCollisionProcessGenerator& operator=(FixStateCollisionProcessGenerator&&) = delete; + ~FixStateCollisionTaskGenerator() override = default; + FixStateCollisionTaskGenerator(const FixStateCollisionTaskGenerator&) = delete; + FixStateCollisionTaskGenerator& operator=(const FixStateCollisionTaskGenerator&) = delete; + FixStateCollisionTaskGenerator(FixStateCollisionTaskGenerator&&) = delete; + FixStateCollisionTaskGenerator& operator=(FixStateCollisionTaskGenerator&&) = delete; FixStateCollisionProfileMap composite_profiles; - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; - void process(ProcessInput input, std::size_t unique_id) const override; + void process(TaskInput input, std::size_t unique_id) const override; }; -class FixStateCollisionProcessInfo : public ProcessInfo +class FixStateCollisionTaskInfo : public TaskInfo { public: - FixStateCollisionProcessInfo(std::size_t unique_id, std::string name = "Fix State Collision"); + FixStateCollisionTaskInfo(std::size_t unique_id, std::string name = "Fix State Collision"); std::vector contact_results; }; @@ -112,7 +112,7 @@ class FixStateCollisionProcessInfo : public ProcessInfo * @return True if in collision */ bool StateInCollision(const Eigen::Ref& start_pos, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile, tesseract_collision::ContactResultMap& contacts); @@ -123,7 +123,7 @@ bool StateInCollision(const Eigen::Ref& start_pos, * @return True if in collision */ bool WaypointInCollision(const Waypoint& waypoint, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile, tesseract_collision::ContactResultMap& contacts); @@ -135,7 +135,7 @@ bool WaypointInCollision(const Waypoint& waypoint, * @return True if successful */ bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile); /** @@ -146,9 +146,9 @@ bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint, * @return True if successful */ bool MoveWaypointFromCollisionRandomSampler(Waypoint& waypoint, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile); -bool ApplyCorrectionWorkflow(Waypoint& waypoint, const ProcessInput& input, const FixStateCollisionProfile& profile); +bool ApplyCorrectionWorkflow(Waypoint& waypoint, const TaskInput& input, const FixStateCollisionProfile& profile); } // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_PROCESS_GENERATOR_H +#endif // TESSERACT_PROCESS_MANAGERS_FIX_STATE_BOUNDS_TASK_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/iterative_spline_parameterization_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/iterative_spline_parameterization_task_generator.h similarity index 58% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/iterative_spline_parameterization_process_generator.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/iterative_spline_parameterization_task_generator.h index 65edb44013e..e4de6422aba 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/iterative_spline_parameterization_process_generator.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/iterative_spline_parameterization_task_generator.h @@ -1,5 +1,5 @@ /** - * @file iterative_spline_parameterization_process_generator.h + * @file iterative_spline_parameterization_task_generator.h * @brief Perform iterative spline time parameterization * * @author Levi Armstrong @@ -23,16 +23,16 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_ITERATIVE_SPLINE_PARAMETERIZATION_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_ITERATIVE_SPLINE_PARAMETERIZATION_PROCESS_GENERATOR_H +#ifndef TESSERACT_PROCESS_MANAGERS_ITERATIVE_SPLINE_PARAMETERIZATION_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_ITERATIVE_SPLINE_PARAMETERIZATION_TASK_GENERATOR_H -#include +#include #include #ifdef SWIG %shared_ptr(tesseract_planning::IterativeSplineParameterizationProfile) -%ignore IterativeSplineParameterizationProcessGenerator; -%ignore IterativeSplineParameterizationProcessInfo; +%ignore IterativeSplineParameterizationTaskGenerator; +%ignore IterativeSplineParameterizationTaskInfo; #endif // SWIG namespace tesseract_planning @@ -54,38 +54,36 @@ struct IterativeSplineParameterizationProfile using IterativeSplineParameterizationProfileMap = std::unordered_map; -class IterativeSplineParameterizationProcessGenerator : public ProcessGenerator +class IterativeSplineParameterizationTaskGenerator : public TaskGenerator { public: - using UPtr = std::unique_ptr; + using UPtr = std::unique_ptr; - IterativeSplineParameterizationProcessGenerator(bool add_points = true, - std::string name = "Iterative Spline Parameterization"); + IterativeSplineParameterizationTaskGenerator(bool add_points = true, + std::string name = "Iterative Spline Parameterization"); - ~IterativeSplineParameterizationProcessGenerator() override = default; - IterativeSplineParameterizationProcessGenerator(const IterativeSplineParameterizationProcessGenerator&) = delete; - IterativeSplineParameterizationProcessGenerator& - operator=(const IterativeSplineParameterizationProcessGenerator&) = delete; - IterativeSplineParameterizationProcessGenerator(IterativeSplineParameterizationProcessGenerator&&) = delete; - IterativeSplineParameterizationProcessGenerator& - operator=(IterativeSplineParameterizationProcessGenerator&&) = delete; + ~IterativeSplineParameterizationTaskGenerator() override = default; + IterativeSplineParameterizationTaskGenerator(const IterativeSplineParameterizationTaskGenerator&) = delete; + IterativeSplineParameterizationTaskGenerator& operator=(const IterativeSplineParameterizationTaskGenerator&) = delete; + IterativeSplineParameterizationTaskGenerator(IterativeSplineParameterizationTaskGenerator&&) = delete; + IterativeSplineParameterizationTaskGenerator& operator=(IterativeSplineParameterizationTaskGenerator&&) = delete; IterativeSplineParameterizationProfileMap composite_profiles; IterativeSplineParameterizationProfileMap move_profiles; - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; - void process(ProcessInput input, std::size_t unique_id) const override; + void process(TaskInput input, std::size_t unique_id) const override; private: IterativeSplineParameterization solver_; }; -class IterativeSplineParameterizationProcessInfo : public ProcessInfo +class IterativeSplineParameterizationTaskInfo : public TaskInfo { public: - IterativeSplineParameterizationProcessInfo(std::size_t unique_id, - std::string name = "Iterative Spline Parameterization"); + IterativeSplineParameterizationTaskInfo(std::size_t unique_id, + std::string name = "Iterative Spline Parameterization"); }; } // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_ITERATIVE_SPLINE_PARAMETERIZATION_PROCESS_GENERATOR_H +#endif // TESSERACT_PROCESS_MANAGERS_ITERATIVE_SPLINE_PARAMETERIZATION_TASK_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/motion_planner_task_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/motion_planner_task_generator.h new file mode 100644 index 00000000000..c1fe1d26926 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/motion_planner_task_generator.h @@ -0,0 +1,64 @@ +/** + * @file motion_planner_task_generator.h + * @brief Generates a motion planning process + * + * @author Matthew Powelson + * @date July 15. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, 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_MOTION_PLANNER_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_MOTION_PLANNER_TASK_GENERATOR_H + +#include + +namespace tesseract_planning +{ +// Forward Declare +class MotionPlanner; + +class MotionPlannerTaskGenerator : public TaskGenerator +{ +public: + using UPtr = std::unique_ptr; + + MotionPlannerTaskGenerator(std::shared_ptr planner); + ~MotionPlannerTaskGenerator() override = default; + MotionPlannerTaskGenerator(const MotionPlannerTaskGenerator&) = delete; + MotionPlannerTaskGenerator& operator=(const MotionPlannerTaskGenerator&) = delete; + MotionPlannerTaskGenerator(MotionPlannerTaskGenerator&&) = delete; + MotionPlannerTaskGenerator& operator=(MotionPlannerTaskGenerator&&) = delete; + + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; + + void process(TaskInput input, std::size_t unique_id) const override; + +private: + std::shared_ptr planner_{ nullptr }; +}; + +class MotionPlannerTaskInfo : public TaskInfo +{ +public: + MotionPlannerTaskInfo(std::size_t unique_id, std::string name = "Motion Planner Process Generator"); +}; + +} // namespace tesseract_planning + +#endif diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/profile_switch_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/profile_switch_task_generator.h similarity index 54% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/profile_switch_process_generator.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/profile_switch_task_generator.h index 6033bcf02cd..6405d53baf5 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/profile_switch_process_generator.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/profile_switch_task_generator.h @@ -1,5 +1,5 @@ /** - * @file profile_switch_process_generator.h + * @file profile_switch_task_generator.h * @brief Process generator that returns a value based on the profile * * @author Matthew Powelson @@ -21,15 +21,15 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_PROCESS_GENERATOR_H +#ifndef TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_TASK_GENERATOR_H -#include +#include #ifdef SWIG %shared_ptr(tesseract_planning::ProfileSwitchProfile) -%ignore ProfileSwitchProcessGenerator; -%ignore ProfileSwitchProcessInfo; +%ignore ProfileSwitchTaskGenerator; +%ignore ProfileSwitchTaskInfo; #endif // SWIG namespace tesseract_planning @@ -49,30 +49,30 @@ using ProfileSwitchProfileMap = std::unordered_map; + using UPtr = std::unique_ptr; - ProfileSwitchProcessGenerator(std::string name = "Profile Switch"); + ProfileSwitchTaskGenerator(std::string name = "Profile Switch"); - ~ProfileSwitchProcessGenerator() override = default; - ProfileSwitchProcessGenerator(const ProfileSwitchProcessGenerator&) = delete; - ProfileSwitchProcessGenerator& operator=(const ProfileSwitchProcessGenerator&) = delete; - ProfileSwitchProcessGenerator(ProfileSwitchProcessGenerator&&) = delete; - ProfileSwitchProcessGenerator& operator=(ProfileSwitchProcessGenerator&&) = delete; + ~ProfileSwitchTaskGenerator() override = default; + ProfileSwitchTaskGenerator(const ProfileSwitchTaskGenerator&) = delete; + ProfileSwitchTaskGenerator& operator=(const ProfileSwitchTaskGenerator&) = delete; + ProfileSwitchTaskGenerator(ProfileSwitchTaskGenerator&&) = delete; + ProfileSwitchTaskGenerator& operator=(ProfileSwitchTaskGenerator&&) = delete; ProfileSwitchProfileMap composite_profiles; - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; - void process(ProcessInput input, std::size_t unique_id) const override; + void process(TaskInput input, std::size_t unique_id) const override; }; -class ProfileSwitchProcessInfo : public ProcessInfo +class ProfileSwitchTaskInfo : public TaskInfo { public: - ProfileSwitchProcessInfo(std::size_t unique_id, std::string name = "Profile Switch"); + ProfileSwitchTaskInfo(std::size_t unique_id, std::string name = "Profile Switch"); }; } // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_PROCESS_GENERATOR_H +#endif // TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_TASK_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/seed_min_length_process_generator.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/seed_min_length_task_generator.h similarity index 53% rename from tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/seed_min_length_process_generator.h rename to tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/seed_min_length_task_generator.h index f8ebc4d1dbb..fb96dd38740 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/seed_min_length_process_generator.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/task_generators/seed_min_length_task_generator.h @@ -1,5 +1,5 @@ /** - * @file seed_length_process_generator.h + * @file seed_length_task_generator.h * @brief Process generator for processing the seed so it meets a minimum length. Planners like trajopt need * at least 10 states in the trajectory to perform velocity, accelleration and jerk smoothing. * @@ -24,36 +24,36 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_PROCESS_MANAGERS_SEED_MIN_LENGTH_PROCESS_GENERATOR_H -#define TESSERACT_PROCESS_MANAGERS_SEED_MIN_LENGTH_PROCESS_GENERATOR_H +#ifndef TESSERACT_PROCESS_MANAGERS_SEED_MIN_LENGTH_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_SEED_MIN_LENGTH_TASK_GENERATOR_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_planning { -class SeedMinLengthProcessGenerator : public ProcessGenerator +class SeedMinLengthTaskGenerator : public TaskGenerator { public: - using UPtr = std::unique_ptr; + using UPtr = std::unique_ptr; - SeedMinLengthProcessGenerator(std::string name = "Seed Min Length"); + SeedMinLengthTaskGenerator(std::string name = "Seed Min Length"); - SeedMinLengthProcessGenerator(long min_length, std::string name = "Seed Min Length"); + SeedMinLengthTaskGenerator(long min_length, std::string name = "Seed Min Length"); - ~SeedMinLengthProcessGenerator() override = default; - SeedMinLengthProcessGenerator(const SeedMinLengthProcessGenerator&) = delete; - SeedMinLengthProcessGenerator& operator=(const SeedMinLengthProcessGenerator&) = delete; - SeedMinLengthProcessGenerator(SeedMinLengthProcessGenerator&&) = delete; - SeedMinLengthProcessGenerator& operator=(SeedMinLengthProcessGenerator&&) = delete; + ~SeedMinLengthTaskGenerator() override = default; + SeedMinLengthTaskGenerator(const SeedMinLengthTaskGenerator&) = delete; + SeedMinLengthTaskGenerator& operator=(const SeedMinLengthTaskGenerator&) = delete; + SeedMinLengthTaskGenerator(SeedMinLengthTaskGenerator&&) = delete; + SeedMinLengthTaskGenerator& operator=(SeedMinLengthTaskGenerator&&) = delete; - int conditionalProcess(ProcessInput input, std::size_t unique_id) const override; + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; - void process(ProcessInput input, std::size_t unique_id) const override; + void process(TaskInput input, std::size_t unique_id) const override; private: long min_length_{ 10 }; @@ -64,11 +64,11 @@ class SeedMinLengthProcessGenerator : public ProcessGenerator int subdivisions) const; }; -class SeedMinLengthProcessInfo : public ProcessInfo +class SeedMinLengthTaskInfo : public TaskInfo { public: - SeedMinLengthProcessInfo(std::size_t unique_id, std::string name = "Seed Min Length"); + SeedMinLengthTaskInfo(std::size_t unique_id, std::string name = "Seed Min Length"); }; } // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_SEED_MIN_LENGTH_PROCESS_GENERATOR_H +#endif // TESSERACT_PROCESS_MANAGERS_SEED_MIN_LENGTH_TASK_GENERATOR_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h index be3f9a1a9e5..fa97c1b06c1 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h @@ -60,18 +60,18 @@ class CartesianTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: std::string name_; CartesianTaskflowParams params_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning #endif // TESSERACT_PROCESS_MANAGERS_CARTESIAN_TASKFLOW_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h index 232236668ee..4ce4dc742e1 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h @@ -60,18 +60,18 @@ class DescartesTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: std::string name_; DescartesTaskflowParams params_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning #endif // TESSERACT_PROCESS_MANAGERS_DESCARTES_TASKFLOW_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h index d2327b37ea2..8b4abbde5e8 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h @@ -67,18 +67,18 @@ class FreespaceTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: std::string name_; FreespaceTaskflowParams params_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning #endif // TESSERACT_PROCESS_MANAGERS_FREESPACE_TASKFLOW_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h index ec1bb2c3bab..a1a1156d697 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include namespace tesseract_planning { @@ -77,7 +77,7 @@ class GraphTaskflow : public TaskflowGenerator struct Node { - ProcessGenerator::UPtr process; + TaskGenerator::UPtr process; NodeType process_type; std::vector edges; }; @@ -91,7 +91,7 @@ class GraphTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; /** * @brief Add a node to the taskflow graph along with setting the process type. @@ -99,7 +99,7 @@ class GraphTaskflow : public TaskflowGenerator * @param process_type The process type assigned to the node * @return The node ID which should be used with adding edges */ - int addNode(ProcessGenerator::UPtr process, NodeType process_type); + int addNode(TaskGenerator::UPtr process, NodeType process_type); /** * @brief Add an edge to the taskflow graph diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h index 6732d0ac07e..d524ff75ec1 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h @@ -59,18 +59,18 @@ class OMPLTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: std::string name_; OMPLTaskflowParams params_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning #endif // TESSERACT_PROCESS_MANAGERS_OMPL_TASKFLOW_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_dt_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_dt_taskflow.h index 3a482ce7fa1..4e46192f4b7 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_dt_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_dt_taskflow.h @@ -41,7 +41,7 @@ namespace tesseract_planning /** * @brief This class provides a taskflow generator for a raster process. * - * Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to + * Given a TaskInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to * execute them efficiently in a parallel based on those dependencies. * * The required format is below. Note that a transition is planned from both the start and end of each raster to allow @@ -83,7 +83,7 @@ class RasterDTTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: TaskflowGenerator::UPtr freespace_taskflow_generator_; @@ -92,11 +92,11 @@ class RasterDTTaskflow : public TaskflowGenerator std::string name_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_global_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_global_taskflow.h index e9924aed883..4dbdee0d82d 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_global_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_global_taskflow.h @@ -41,7 +41,7 @@ namespace tesseract_planning /** * @brief This class provides a process manager for a raster process. * - * Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to + * Given a TaskInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to * execute them efficiently in a parallel based on those dependencies. * * The required format is below. @@ -77,7 +77,7 @@ class RasterGlobalTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: TaskflowGenerator::UPtr global_taskflow_generator_; @@ -86,14 +86,14 @@ class RasterGlobalTaskflow : public TaskflowGenerator TaskflowGenerator::UPtr raster_taskflow_generator_; std::string name_; - static void globalPostProcess(ProcessInput input); + static void globalPostProcess(TaskInput input); /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_global_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_global_taskflow.h index f61c7a99d69..823da7a7731 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_global_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_global_taskflow.h @@ -41,7 +41,7 @@ namespace tesseract_planning /** * @brief This class provides a taskflow for a raster process. * - * Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to + * Given a TaskInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to * execute them efficiently in a parallel based on those dependencies. * * The required format is below. @@ -74,7 +74,7 @@ class RasterOnlyGlobalTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: TaskflowGenerator::UPtr global_taskflow_generator_; @@ -82,14 +82,14 @@ class RasterOnlyGlobalTaskflow : public TaskflowGenerator TaskflowGenerator::UPtr raster_taskflow_generator_; std::string name_; - static void globalPostProcess(ProcessInput input); + static void globalPostProcess(TaskInput input); /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_taskflow.h index 742cc408670..f019674d41f 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_only_taskflow.h @@ -41,7 +41,7 @@ namespace tesseract_planning /** * @brief This class provides a taskflow for a raster process. * - * Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to + * Given a TaskInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to * execute them efficiently in a parallel based on those dependencies. * * The required format is below. @@ -73,7 +73,7 @@ class RasterOnlyTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: TaskflowGenerator::UPtr transition_taskflow_generator_; @@ -81,11 +81,11 @@ class RasterOnlyTaskflow : public TaskflowGenerator std::string name_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_taskflow.h index 535dc6ff2c6..433c774970d 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_taskflow.h @@ -41,7 +41,7 @@ namespace tesseract_planning /** * @brief This class provides a taskflow for a raster process. * - * Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to + * Given a TaskInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to * execute them efficiently in a parallel based on those dependencies. * * The required format is below. @@ -76,7 +76,7 @@ class RasterTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: TaskflowGenerator::UPtr freespace_taskflow_generator_; @@ -85,11 +85,11 @@ class RasterTaskflow : public TaskflowGenerator std::string name_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_dt_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_dt_taskflow.h index c600ddfe842..2a85d0feef8 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_dt_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_dt_taskflow.h @@ -41,7 +41,7 @@ namespace tesseract_planning /** * @brief This class provides a taskflow for a raster process. * - * Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to + * Given a TaskInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to * execute them efficiently in a parallel based on those dependencies. * * The required format is below. Note that a transition is planned from both the start and end of each raster to allow @@ -98,7 +98,7 @@ class RasterWAADDTTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: TaskflowGenerator::UPtr freespace_taskflow_generator_; @@ -107,11 +107,11 @@ class RasterWAADDTTaskflow : public TaskflowGenerator std::string name_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_taskflow.h index 457c484413b..4a50c684ec7 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/raster_waad_taskflow.h @@ -42,7 +42,7 @@ namespace tesseract_planning /** * @brief This class provides a process manager for a raster process. * - * Given a ProcessInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to + * Given a TaskInput in the correct format, it handles the creation of the process dependencies and uses Taskflow to * execute them efficiently in a parallel based on those dependencies. * * The required format is below. Note that a transition is planned from both the start and end of each raster to allow @@ -91,7 +91,7 @@ class RasterWAADTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: TaskflowGenerator::UPtr freespace_taskflow_generator_; @@ -100,11 +100,11 @@ class RasterWAADTaskflow : public TaskflowGenerator std::string name_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h index b83455ca8e3..5c0a8a84278 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h +++ b/tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h @@ -60,18 +60,18 @@ class TrajOptTaskflow : public TaskflowGenerator const std::string& getName() const override; - TaskflowContainer generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; + TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; private: std::string name_; TrajOptTaskflowParams params_; /** - * @brief Checks that the ProcessInput is in the correct format. - * @param input ProcessInput to be checked + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked * @return True if in the correct format */ - bool checkProcessInput(const ProcessInput& input) const; + bool checkTaskInput(const TaskInput& input) const; }; } // namespace tesseract_planning #endif // TESSERACT_PROCESS_MANAGERS_TRAJOPT_TASKFLOW_H diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp index f94f33c4f3e..5d765612b77 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_planning_server.cpp @@ -29,6 +29,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include @@ -147,7 +148,7 @@ ProcessPlanningFuture ProcessPlanningServer::run(const ProcessPlanningRequest& r // This makes sure the Joint and State Waypoints match the same order as the kinematics if (formatProgram(*composite_program, *tc)) { - CONSOLE_BRIDGE_logInform("Tesseract Planning Server: Input program required formating!"); + CONSOLE_BRIDGE_logInform("Tesseract Planning Server: Input program required formatting!"); } if (!request.commands.empty() && !tc->applyCommands(request.commands)) @@ -156,16 +157,16 @@ ProcessPlanningFuture ProcessPlanningServer::run(const ProcessPlanningRequest& r return response; } - ProcessInput process_input(tc, - response.input.get(), - *(response.global_manip_info), - *(response.plan_profile_remapping), - *(response.composite_profile_remapping), - response.results.get(), - has_seed, - profiles_); - response.interface = process_input.getProcessInterface(); - response.taskflow_container = it->second->generateTaskflow(process_input, nullptr, nullptr); + TaskInput task_input(tc, + response.input.get(), + *(response.global_manip_info), + *(response.plan_profile_remapping), + *(response.composite_profile_remapping), + response.results.get(), + has_seed, + profiles_); + response.interface = task_input.getTaskInterface(); + response.taskflow_container = it->second->generateTaskflow(task_input, nullptr, nullptr); // Dump taskflow graph before running if (console_bridge::getLogLevel() >= console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/core/task_generator.cpp similarity index 72% rename from tesseract/tesseract_planning/tesseract_process_managers/src/core/process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/core/task_generator.cpp index 78079de7df9..df75d3a99d4 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/core/task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file process_generator.cpp + * @file task_generator.cpp * @brief Process generator * * @author Matthew Powelson @@ -24,15 +24,15 @@ * limitations under the License. */ -#include +#include namespace tesseract_planning { -ProcessGenerator::ProcessGenerator(std::string name) : name_(std::move(name)) {} +TaskGenerator::TaskGenerator(std::string name) : name_(std::move(name)) {} -const std::string& ProcessGenerator::getName() const { return name_; } +const std::string& TaskGenerator::getName() const { return name_; } -tf::Task ProcessGenerator::generateTask(ProcessInput input, tf::Taskflow& taskflow) +tf::Task TaskGenerator::generateTask(TaskInput input, tf::Taskflow& taskflow) { tf::Task task = taskflow.placeholder(); std::size_t unique_id = task.hash_value(); @@ -41,14 +41,14 @@ tf::Task ProcessGenerator::generateTask(ProcessInput input, tf::Taskflow& taskfl return task; } -void ProcessGenerator::assignTask(ProcessInput input, tf::Task& task) +void TaskGenerator::assignTask(TaskInput input, tf::Task& task) { std::size_t unique_id = task.hash_value(); task.work([=]() { process(input, unique_id); }); task.name(getName()); } -tf::Task ProcessGenerator::generateConditionalTask(ProcessInput input, tf::Taskflow& taskflow) +tf::Task TaskGenerator::generateConditionalTask(TaskInput input, tf::Taskflow& taskflow) { tf::Task task = taskflow.placeholder(); std::size_t unique_id = task.hash_value(); @@ -57,7 +57,7 @@ tf::Task ProcessGenerator::generateConditionalTask(ProcessInput input, tf::Taskf return task; } -void ProcessGenerator::assignConditionalTask(ProcessInput input, tf::Task& task) +void TaskGenerator::assignConditionalTask(TaskInput input, tf::Task& task) { std::size_t unique_id = task.hash_value(); task.work([=]() { return conditionalProcess(input, unique_id); }); diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_info.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/core/task_info.cpp similarity index 64% rename from tesseract/tesseract_planning/tesseract_process_managers/src/core/process_info.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/core/task_info.cpp index d683c3af10f..5ede20ff467 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_info.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/core/task_info.cpp @@ -1,5 +1,5 @@ /** - * @file process_info.h + * @file task_info.h * @brief Process Info * * @author Matthew Powelson @@ -24,28 +24,28 @@ * limitations under the License. */ -#include +#include namespace tesseract_planning { -ProcessInfo::ProcessInfo(std::size_t unique_id, std::string name) : unique_id(unique_id), message(std::move(name)) {} +TaskInfo::TaskInfo(std::size_t unique_id, std::string name) : unique_id(unique_id), message(std::move(name)) {} -void ProcessInfoContainer::addProcessInfo(ProcessInfo::ConstPtr process_info) +void TaskInfoContainer::addTaskInfo(TaskInfo::ConstPtr task_info) { std::unique_lock lock(mutex_); - process_info_map_[process_info->unique_id] = std::move(process_info); + task_info_map_[task_info->unique_id] = std::move(task_info); } -ProcessInfo::ConstPtr ProcessInfoContainer::operator[](std::size_t index) const +TaskInfo::ConstPtr TaskInfoContainer::operator[](std::size_t index) const { std::shared_lock lock(mutex_); - return process_info_map_.at(index); + return task_info_map_.at(index); } -std::map ProcessInfoContainer::getProcessInfoMap() const +std::map TaskInfoContainer::getTaskInfoMap() const { std::shared_lock lock(mutex_); - return process_info_map_; + return task_info_map_; } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_input.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/core/task_input.cpp similarity index 63% rename from tesseract/tesseract_planning/tesseract_process_managers/src/core/process_input.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/core/task_input.cpp index 7d0d49f77b7..21fbc7990e6 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_input.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/core/task_input.cpp @@ -1,5 +1,5 @@ /** - * @file process_input.cpp + * @file task_input.cpp * @brief Process input * * @author Matthew Powelson @@ -30,7 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include #include @@ -40,12 +40,12 @@ namespace tesseract_planning static const ManipulatorInfo EMPTY_MANIPULATOR_INFO; static const PlannerProfileRemapping EMPTY_PROFILE_MAPPING; -ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - const ManipulatorInfo& manip_info, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles) +TaskInput::TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + const ManipulatorInfo& manip_info, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles) : env(std::move(env)) , manip_info(manip_info) , plan_profile_remapping(EMPTY_PROFILE_MAPPING) @@ -57,14 +57,14 @@ ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, { } -ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - const ManipulatorInfo& manip_info, - const PlannerProfileRemapping& plan_profile_remapping, - const PlannerProfileRemapping& composite_profile_remapping, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles) +TaskInput::TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + const ManipulatorInfo& manip_info, + const PlannerProfileRemapping& plan_profile_remapping, + const PlannerProfileRemapping& composite_profile_remapping, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles) : env(std::move(env)) , manip_info(manip_info) , plan_profile_remapping(plan_profile_remapping) @@ -76,13 +76,13 @@ ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, { } -ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - const PlannerProfileRemapping& plan_profile_remapping, - const PlannerProfileRemapping& composite_profile_remapping, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles) +TaskInput::TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + const PlannerProfileRemapping& plan_profile_remapping, + const PlannerProfileRemapping& composite_profile_remapping, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles) : env(std::move(env)) , manip_info(EMPTY_MANIPULATOR_INFO) , plan_profile_remapping(plan_profile_remapping) @@ -94,11 +94,11 @@ ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, { } -ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, - const Instruction* instruction, - Instruction* seed, - bool has_seed, - ProfileDictionary::ConstPtr profiles) +TaskInput::TaskInput(tesseract_environment::Environment::ConstPtr env, + const Instruction* instruction, + Instruction* seed, + bool has_seed, + ProfileDictionary::ConstPtr profiles) : env(std::move(env)) , manip_info(EMPTY_MANIPULATOR_INFO) , plan_profile_remapping(EMPTY_PROFILE_MAPPING) @@ -110,15 +110,15 @@ ProcessInput::ProcessInput(tesseract_environment::Environment::ConstPtr env, { } -ProcessInput ProcessInput::operator[](std::size_t index) +TaskInput TaskInput::operator[](std::size_t index) { - ProcessInput pi(*this); + TaskInput pi(*this); pi.instruction_indice_.push_back(index); return pi; } -std::size_t ProcessInput::size() +std::size_t TaskInput::size() { const Instruction* ci = instruction_; for (const auto& i : instruction_indice_) @@ -143,7 +143,7 @@ std::size_t ProcessInput::size() return 0; } -const Instruction* ProcessInput::getInstruction() const +const Instruction* TaskInput::getInstruction() const { const Instruction* ci = instruction_; for (const auto& i : instruction_indice_) @@ -161,7 +161,7 @@ const Instruction* ProcessInput::getInstruction() const return ci; } -Instruction* ProcessInput::getResults() +Instruction* TaskInput::getResults() { Instruction* ci = results_; for (const auto& i : instruction_indice_) @@ -179,25 +179,25 @@ Instruction* ProcessInput::getResults() return ci; } -ProcessInterface::Ptr ProcessInput::getProcessInterface() { return interface_; } +TaskflowInterface::Ptr TaskInput::getTaskInterface() { return interface_; } -bool ProcessInput::isAborted() const { return interface_->isAborted(); } +bool TaskInput::isAborted() const { return interface_->isAborted(); } -void ProcessInput::abort() { interface_->abort(); } +void TaskInput::abort() { interface_->abort(); } -void ProcessInput::setStartInstruction(Instruction start) +void TaskInput::setStartInstruction(Instruction start) { start_instruction_ = start; start_instruction_indice_.clear(); } -void ProcessInput::setStartInstruction(std::vector start) +void TaskInput::setStartInstruction(std::vector start) { start_instruction_indice_ = start; start_instruction_ = NullInstruction(); } -Instruction ProcessInput::getStartInstruction() const +Instruction TaskInput::getStartInstruction() const { if (!isNullInstruction(start_instruction_)) return start_instruction_; @@ -225,19 +225,19 @@ Instruction ProcessInput::getStartInstruction() const return *ci; } -void ProcessInput::setEndInstruction(Instruction end) +void TaskInput::setEndInstruction(Instruction end) { end_instruction_ = end; end_instruction_indice_.clear(); } -void ProcessInput::setEndInstruction(std::vector end) +void TaskInput::setEndInstruction(std::vector end) { end_instruction_indice_ = end; end_instruction_ = NullInstruction(); } -Instruction ProcessInput::getEndInstruction() const +Instruction TaskInput::getEndInstruction() const { if (!isNullInstruction(end_instruction_)) return end_instruction_; @@ -268,15 +268,18 @@ Instruction ProcessInput::getEndInstruction() const return *ci; } -void ProcessInput::addProcessInfo(const ProcessInfo::ConstPtr& process_info) +void TaskInput::addTaskInfo(const TaskInfo::ConstPtr& task_info) { - process_infos_->addProcessInfo(process_info); + interface_->getTaskInfoContainer()->addTaskInfo(task_info); } -ProcessInfo::ConstPtr ProcessInput::getProcessInfo(const std::size_t& index) const { return (*process_infos_)[index]; } +TaskInfo::ConstPtr TaskInput::getTaskInfo(const std::size_t& index) const +{ + return (*interface_->getTaskInfoContainer())[index]; +} -std::map ProcessInput::getProcessInfoMap() const +std::map TaskInput::getTaskInfoMap() const { - return process_infos_->getProcessInfoMap(); + return interface_->getTaskInfoContainer()->getTaskInfoMap(); } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_interface.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/core/taskflow_interface.cpp similarity index 55% rename from tesseract/tesseract_planning/tesseract_process_managers/src/core/process_interface.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/core/taskflow_interface.cpp index 7a78e2a682b..b5bcf5d0e74 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/core/process_interface.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/core/taskflow_interface.cpp @@ -1,5 +1,5 @@ /** - * @file process_interface.h + * @file taskflow_interface.h * @brief Process Inteface * * @author Levi Armstrong @@ -24,13 +24,29 @@ * limitations under the License. */ -#include +#include +#include namespace tesseract_planning { -bool ProcessInterface::isAborted() const { return abort_; } +bool TaskflowInterface::isAborted() const { return abort_; } -bool ProcessInterface::isSuccessful() const { return !abort_; } +bool TaskflowInterface::isSuccessful() const { return !abort_; } + +void TaskflowInterface::abort() { abort_ = true; } + +TaskInfo::ConstPtr TaskflowInterface::getTaskInfo(const std::size_t& index) const +{ + if (task_infos_) + return (*task_infos_)[index]; + return nullptr; +} + +std::map TaskflowInterface::getTaskInfoMap() const +{ + return task_infos_->getTaskInfoMap(); +} + +TaskInfoContainer::Ptr TaskflowInterface::getTaskInfoContainer() const { return task_infos_; } -void ProcessInterface::abort() { abort_ = true; } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/core/utils.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/core/utils.cpp index 4be67e76ee4..2e2a22e757e 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/core/utils.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/core/utils.cpp @@ -29,7 +29,7 @@ namespace tesseract_planning { -void successTask(const ProcessInput& /*instruction*/, +void successTask(const TaskInput& /*instruction*/, const std::string& name, const std::string& message, const TaskflowVoidFn& user_callback) @@ -39,7 +39,7 @@ void successTask(const ProcessInput& /*instruction*/, user_callback(); } -void failureTask(ProcessInput instruction, +void failureTask(TaskInput instruction, const std::string& name, const std::string& message, const TaskflowVoidFn& user_callback) @@ -71,7 +71,7 @@ bool isCompositeEmpty(const CompositeInstruction& composite) return false; } -int hasSeedTask(ProcessInput input) +int hasSeedTask(TaskInput input) { if (input.has_seed) return 1; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/continuous_contact_check_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/continuous_contact_check_task_generator.cpp similarity index 77% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/continuous_contact_check_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/continuous_contact_check_task_generator.cpp index a02f5ca8f4d..0270493f603 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/continuous_contact_check_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/continuous_contact_check_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file continuous_contact_check_process_generator.cpp + * @file continuous_contact_check_task_generator.cpp * @brief Continuous collision check trajectory * * @author Levi Armstrong @@ -29,43 +29,43 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include namespace tesseract_planning { -ContinuousContactCheckProcessGenerator::ContinuousContactCheckProcessGenerator(std::string name) - : ProcessGenerator(std::move(name)) +ContinuousContactCheckTaskGenerator::ContinuousContactCheckTaskGenerator(std::string name) + : TaskGenerator(std::move(name)) { config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; config.longest_valid_segment_length = 0.05; config.collision_margin_data = tesseract_collision::CollisionMarginData(0); } -ContinuousContactCheckProcessGenerator::ContinuousContactCheckProcessGenerator(double longest_valid_segment_length, - double contact_distance, - std::string name) - : ProcessGenerator(std::move(name)) +ContinuousContactCheckTaskGenerator::ContinuousContactCheckTaskGenerator(double longest_valid_segment_length, + double contact_distance, + std::string name) + : TaskGenerator(std::move(name)) { config.longest_valid_segment_length = longest_valid_segment_length; config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; config.collision_margin_data = tesseract_collision::CollisionMarginData(contact_distance); if (config.longest_valid_segment_length <= 0) { - CONSOLE_BRIDGE_logWarn("ContinuousContactCheckProcessGenerator: Invalid longest valid segment. Defaulting to 0.05"); + CONSOLE_BRIDGE_logWarn("ContinuousContactCheckTaskGenerator: Invalid longest valid segment. Defaulting to 0.05"); config.longest_valid_segment_length = 0.05; } } -int ContinuousContactCheckProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int ContinuousContactCheckTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // -------------------- // Check that inputs are valid @@ -73,7 +73,7 @@ int ContinuousContactCheckProcessGenerator::conditionalProcess(ProcessInput inpu Instruction* input_results = input.getResults(); if (!isCompositeInstruction(*input_results)) { - info->message = "Input seed to ContinuousContactCheckProcessGenerator must be a composite instruction"; + info->message = "Input seed to ContinuousContactCheckTaskGenerator must be a composite instruction"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); return 0; } @@ -117,13 +117,13 @@ int ContinuousContactCheckProcessGenerator::conditionalProcess(ProcessInput inpu return 1; } -void ContinuousContactCheckProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void ContinuousContactCheckTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -ContinuousContactCheckProcessInfo::ContinuousContactCheckProcessInfo(std::size_t unique_id, std::string name) - : ProcessInfo(unique_id, std::move(name)) +ContinuousContactCheckTaskInfo::ContinuousContactCheckTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) { } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/discrete_contact_check_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/discrete_contact_check_task_generator.cpp similarity index 78% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/discrete_contact_check_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/discrete_contact_check_task_generator.cpp index f2f93af9291..5f31beaf79b 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/discrete_contact_check_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/discrete_contact_check_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file discrete_contact_check_process_generator.cpp + * @file discrete_contact_check_task_generator.cpp * @brief Discrete collision check trajectory * * @author Levi Armstrong @@ -29,43 +29,42 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include namespace tesseract_planning { -DiscreteContactCheckProcessGenerator::DiscreteContactCheckProcessGenerator(std::string name) - : ProcessGenerator(std::move(name)) +DiscreteContactCheckTaskGenerator::DiscreteContactCheckTaskGenerator(std::string name) : TaskGenerator(std::move(name)) { config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; config.longest_valid_segment_length = 0.05; config.collision_margin_data = tesseract_collision::CollisionMarginData(0); } -DiscreteContactCheckProcessGenerator::DiscreteContactCheckProcessGenerator(double longest_valid_segment_length, - double contact_distance, - std::string name) - : ProcessGenerator(std::move(name)) +DiscreteContactCheckTaskGenerator::DiscreteContactCheckTaskGenerator(double longest_valid_segment_length, + double contact_distance, + std::string name) + : TaskGenerator(std::move(name)) { config.longest_valid_segment_length = longest_valid_segment_length; config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; config.collision_margin_data = tesseract_collision::CollisionMarginData(contact_distance); if (config.longest_valid_segment_length <= 0) { - CONSOLE_BRIDGE_logWarn("DiscreteContactCheckProcessGenerator: Invalid longest valid segment. Defaulting to 0.05"); + CONSOLE_BRIDGE_logWarn("DiscreteContactCheckTaskGenerator: Invalid longest valid segment. Defaulting to 0.05"); config.longest_valid_segment_length = 0.05; } } -int DiscreteContactCheckProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int DiscreteContactCheckTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // -------------------- // Check that inputs are valid @@ -73,7 +72,7 @@ int DiscreteContactCheckProcessGenerator::conditionalProcess(ProcessInput input, Instruction* input_result = input.getResults(); if (!isCompositeInstruction(*input_result)) { - info->message = "Input seed to DiscreteContactCheckProcessGenerator must be a composite instruction"; + info->message = "Input seed to DiscreteContactCheckTaskGenerator must be a composite instruction"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); return 0; } @@ -117,13 +116,13 @@ int DiscreteContactCheckProcessGenerator::conditionalProcess(ProcessInput input, return 1; } -void DiscreteContactCheckProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void DiscreteContactCheckTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -DiscreteContactCheckProcessInfo::DiscreteContactCheckProcessInfo(std::size_t unique_id, std::string name) - : ProcessInfo(unique_id, std::move(name)) +DiscreteContactCheckTaskInfo::DiscreteContactCheckTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) { } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_bounds_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/fix_state_bounds_task_generator.cpp similarity index 81% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_bounds_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/fix_state_bounds_task_generator.cpp index 34903e51adb..baf44d2ce5c 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_bounds_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/fix_state_bounds_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file fix_state_bounds_process_generator.cpp + * @file fix_state_bounds_task_generator.cpp * @brief Process generator that changes the plan instructions to make push them back within joint limits * * @author Matthew Powelson @@ -29,26 +29,26 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include namespace tesseract_planning { -FixStateBoundsProcessGenerator::FixStateBoundsProcessGenerator(std::string name) : ProcessGenerator(std::move(name)) +FixStateBoundsTaskGenerator::FixStateBoundsTaskGenerator(std::string name) : TaskGenerator(std::move(name)) { // Register default profile composite_profiles["DEFAULT"] = std::make_shared(); } -int FixStateBoundsProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int FixStateBoundsTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // -------------------- // Check that inputs are valid @@ -106,7 +106,7 @@ int FixStateBoundsProcessGenerator::conditionalProcess(ProcessInput input, std:: PlanInstruction* mutable_instruction = const_cast(instr_const_ptr); if (!isWithinJointLimits(mutable_instruction->getWaypoint(), limits)) { - CONSOLE_BRIDGE_logInform("FixStateBoundsProcessGenerator is modifying the const input instructions"); + CONSOLE_BRIDGE_logInform("FixStateBoundsTaskGenerator is modifying the const input instructions"); if (!clampToJointLimits( mutable_instruction->getWaypoint(), limits, cur_composite_profile->max_deviation_global)) return 0; @@ -122,7 +122,7 @@ int FixStateBoundsProcessGenerator::conditionalProcess(ProcessInput input, std:: PlanInstruction* mutable_instruction = const_cast(instr_const_ptr); if (!isWithinJointLimits(mutable_instruction->getWaypoint(), limits)) { - CONSOLE_BRIDGE_logInform("FixStateBoundsProcessGenerator is modifying the const input instructions"); + CONSOLE_BRIDGE_logInform("FixStateBoundsTaskGenerator is modifying the const input instructions"); if (!clampToJointLimits( mutable_instruction->getWaypoint(), limits, cur_composite_profile->max_deviation_global)) return 0; @@ -135,7 +135,7 @@ int FixStateBoundsProcessGenerator::conditionalProcess(ProcessInput input, std:: auto flattened = flatten(*ci, planFilter); if (flattened.empty()) { - CONSOLE_BRIDGE_logWarn("FixStateBoundsProcessGenerator found no PlanInstructions to process"); + CONSOLE_BRIDGE_logWarn("FixStateBoundsTaskGenerator found no PlanInstructions to process"); info->return_value = 1; return 1; } @@ -148,7 +148,7 @@ int FixStateBoundsProcessGenerator::conditionalProcess(ProcessInput input, std:: if (!outside_limits) break; - CONSOLE_BRIDGE_logInform("FixStateBoundsProcessGenerator is modifying the const input instructions"); + CONSOLE_BRIDGE_logInform("FixStateBoundsTaskGenerator is modifying the const input instructions"); for (const auto& instruction : flattened) { const Instruction* instr_const_ptr = &instruction.get(); @@ -164,18 +164,18 @@ int FixStateBoundsProcessGenerator::conditionalProcess(ProcessInput input, std:: return 1; } - CONSOLE_BRIDGE_logDebug("FixStateBoundsProcessGenerator succeeded"); + CONSOLE_BRIDGE_logDebug("FixStateBoundsTaskGenerator succeeded"); info->return_value = 1; return 1; } -void FixStateBoundsProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void FixStateBoundsTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -FixStateBoundsProcessInfo::FixStateBoundsProcessInfo(std::size_t unique_id, std::string name) - : ProcessInfo(unique_id, std::move(name)) +FixStateBoundsTaskInfo::FixStateBoundsTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) { } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_collision_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/fix_state_collision_task_generator.cpp similarity index 90% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_collision_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/fix_state_collision_task_generator.cpp index c2afd46b942..8775cc5b0f4 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_collision_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/fix_state_collision_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file fix_state_collisions_process_generator.cpp + * @file fix_state_collisions_task_generator.cpp * @brief Process generator for process that pushes plan instructions to be out of collision * * @author Matthew Powelson @@ -32,14 +32,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include #include namespace tesseract_planning { bool StateInCollision(const Eigen::Ref& start_pos, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile, tesseract_collision::ContactResultMap& contacts) { @@ -84,7 +84,7 @@ bool StateInCollision(const Eigen::Ref& start_pos, } bool WaypointInCollision(const Waypoint& waypoint, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile, tesseract_collision::ContactResultMap& contacts) { @@ -103,7 +103,7 @@ bool WaypointInCollision(const Waypoint& waypoint, } bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile) { using namespace trajopt; @@ -197,7 +197,7 @@ bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint, } bool MoveWaypointFromCollisionRandomSampler(Waypoint& waypoint, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile) { // Get position associated with waypoint @@ -240,7 +240,7 @@ bool MoveWaypointFromCollisionRandomSampler(Waypoint& waypoint, } bool ApplyCorrectionWorkflow(Waypoint& waypoint, - const ProcessInput& input, + const TaskInput& input, const FixStateCollisionProfile& profile, tesseract_collision::ContactResultMap& contacts) { @@ -265,8 +265,7 @@ bool ApplyCorrectionWorkflow(Waypoint& waypoint, return false; } -FixStateCollisionProcessGenerator::FixStateCollisionProcessGenerator(std::string name) - : ProcessGenerator(std::move(name)) +FixStateCollisionTaskGenerator::FixStateCollisionTaskGenerator(std::string name) : TaskGenerator(std::move(name)) { // Register default profile auto default_profile = std::make_shared(); @@ -275,14 +274,14 @@ FixStateCollisionProcessGenerator::FixStateCollisionProcessGenerator(std::string composite_profiles["DEFAULT"] = default_profile; } -int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int FixStateCollisionTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // -------------------- // Check that inputs are valid @@ -335,7 +334,7 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input, st if (WaypointInCollision( mutable_instruction->getWaypoint(), input, *cur_composite_profile, info->contact_results[0])) { - CONSOLE_BRIDGE_logInform("FixStateCollisionProcessGenerator is modifying the const input instructions"); + CONSOLE_BRIDGE_logInform("FixStateCollisionTaskGenerator is modifying the const input instructions"); if (!ApplyCorrectionWorkflow( mutable_instruction->getWaypoint(), input, *cur_composite_profile, info->contact_results[0])) return 0; @@ -353,7 +352,7 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input, st if (WaypointInCollision( mutable_instruction->getWaypoint(), input, *cur_composite_profile, info->contact_results[0])) { - CONSOLE_BRIDGE_logInform("FixStateCollisionProcessGenerator is modifying the const input instructions"); + CONSOLE_BRIDGE_logInform("FixStateCollisionTaskGenerator is modifying the const input instructions"); if (!ApplyCorrectionWorkflow( mutable_instruction->getWaypoint(), input, *cur_composite_profile, info->contact_results[0])) return 0; @@ -367,7 +366,7 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input, st info->contact_results.resize(flattened.size()); if (flattened.empty()) { - CONSOLE_BRIDGE_logWarn("FixStateCollisionProcessGenerator found no PlanInstructions to process"); + CONSOLE_BRIDGE_logWarn("FixStateCollisionTaskGenerator found no PlanInstructions to process"); info->return_value = 1; return 1; } @@ -383,7 +382,7 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input, st if (!in_collision) break; - CONSOLE_BRIDGE_logInform("FixStateCollisionProcessGenerator is modifying the const input instructions"); + CONSOLE_BRIDGE_logInform("FixStateCollisionTaskGenerator is modifying the const input instructions"); for (std::size_t i = 0; i < flattened.size(); i++) { const Instruction* instr_const_ptr = &flattened[i].get(); @@ -400,18 +399,18 @@ int FixStateCollisionProcessGenerator::conditionalProcess(ProcessInput input, st return 1; } - CONSOLE_BRIDGE_logDebug("FixStateCollisionProcessGenerator succeeded"); + CONSOLE_BRIDGE_logDebug("FixStateCollisionTaskGenerator succeeded"); info->return_value = 1; return 1; } -void FixStateCollisionProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void FixStateCollisionTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -FixStateCollisionProcessInfo::FixStateCollisionProcessInfo(std::size_t unique_id, std::string name) - : ProcessInfo(unique_id, std::move(name)) +FixStateCollisionTaskInfo::FixStateCollisionTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) { } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/iterative_spline_parameterization_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/iterative_spline_parameterization_task_generator.cpp similarity index 85% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/iterative_spline_parameterization_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/iterative_spline_parameterization_task_generator.cpp index 52672f8b4fc..d9dc0b4a16f 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/iterative_spline_parameterization_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/iterative_spline_parameterization_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file iterative_spline_parameterization_process_generator.cpp + * @file iterative_spline_parameterization_task_generator.cpp * @brief Perform iterative spline time parameterization * * @author Levi Armstrong @@ -30,7 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include #include #include #include @@ -46,22 +46,22 @@ IterativeSplineParameterizationProfile::IterativeSplineParameterizationProfile(d { } -IterativeSplineParameterizationProcessGenerator::IterativeSplineParameterizationProcessGenerator(bool add_points, - std::string name) - : ProcessGenerator(std::move(name)), solver_(add_points) +IterativeSplineParameterizationTaskGenerator::IterativeSplineParameterizationTaskGenerator(bool add_points, + std::string name) + : TaskGenerator(std::move(name)), solver_(add_points) { // Register default profile composite_profiles["DEFAULT"] = std::make_shared(); } -int IterativeSplineParameterizationProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int IterativeSplineParameterizationTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // -------------------- // Check that inputs are valid @@ -135,14 +135,14 @@ int IterativeSplineParameterizationProcessGenerator::conditionalProcess(ProcessI return 1; } -void IterativeSplineParameterizationProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void IterativeSplineParameterizationTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -IterativeSplineParameterizationProcessInfo::IterativeSplineParameterizationProcessInfo(std::size_t unique_id, - std::string name) - : ProcessInfo(unique_id, std::move(name)) +IterativeSplineParameterizationTaskInfo::IterativeSplineParameterizationTaskInfo(std::size_t unique_id, + std::string name) + : TaskInfo(unique_id, std::move(name)) { } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/motion_planner_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/motion_planner_task_generator.cpp similarity index 85% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/motion_planner_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/motion_planner_task_generator.cpp index 70dc708c927..5b275e56630 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/motion_planner_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/motion_planner_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file motion_planner_process_generator.cpp + * @file motion_planner_task_generator.cpp * @brief Perform motion planning * * @author Levi Armstrong @@ -29,26 +29,26 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include #include namespace tesseract_planning { -MotionPlannerProcessGenerator::MotionPlannerProcessGenerator(std::shared_ptr planner) - : ProcessGenerator(planner->getName()), planner_(planner) +MotionPlannerTaskGenerator::MotionPlannerTaskGenerator(std::shared_ptr planner) + : TaskGenerator(planner->getName()), planner_(planner) { } -int MotionPlannerProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int MotionPlannerTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // -------------------- // Check that inputs are valid @@ -56,8 +56,7 @@ int MotionPlannerProcessGenerator::conditionalProcess(ProcessInput input, std::s const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - info->message = - "Input instructions to MotionPlannerProcessGenerator: " + name_ + " must be a composite instruction"; + info->message = "Input instructions to MotionPlannerTaskGenerator: " + name_ + " must be a composite instruction"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); return 0; } @@ -65,7 +64,7 @@ int MotionPlannerProcessGenerator::conditionalProcess(ProcessInput input, std::s Instruction* input_results = input.getResults(); if (!isCompositeInstruction(*input_results)) { - info->message = "Input seed to MotionPlannerProcessGenerator: " + name_ + " must be a composite instruction"; + info->message = "Input seed to MotionPlannerTaskGenerator: " + name_ + " must be a composite instruction"; CONSOLE_BRIDGE_logError("%s", info->message.c_str()); return 0; } @@ -179,13 +178,13 @@ int MotionPlannerProcessGenerator::conditionalProcess(ProcessInput input, std::s return 0; } -void MotionPlannerProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void MotionPlannerTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -MotionPlannerProcessInfo::MotionPlannerProcessInfo(std::size_t unique_id, std::string name) - : ProcessInfo(unique_id, std::move(name)) +MotionPlannerTaskInfo::MotionPlannerTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) { } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/profile_switch_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/profile_switch_task_generator.cpp similarity index 78% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/profile_switch_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/profile_switch_task_generator.cpp index d61ca24d771..3234120aed6 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/profile_switch_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/profile_switch_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file profile_switch_process_generator.h + * @file profile_switch_task_generator.h * @brief Process generator that returns a value based on the profile * * @author Matthew Powelson @@ -27,7 +27,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include #include @@ -36,20 +36,20 @@ namespace tesseract_planning { ProfileSwitchProfile::ProfileSwitchProfile(const int& return_value) : return_value(return_value) {} -ProfileSwitchProcessGenerator::ProfileSwitchProcessGenerator(std::string name) : ProcessGenerator(std::move(name)) +ProfileSwitchTaskGenerator::ProfileSwitchTaskGenerator(std::string name) : TaskGenerator(std::move(name)) { // Register default profile composite_profiles[DEFAULT_PROFILE_KEY] = std::make_shared(); } -int ProfileSwitchProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int ProfileSwitchTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // -------------------- // Check that inputs are valid @@ -79,13 +79,13 @@ int ProfileSwitchProcessGenerator::conditionalProcess(ProcessInput input, std::s return cur_composite_profile->return_value; } -void ProfileSwitchProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void ProfileSwitchTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -ProfileSwitchProcessInfo::ProfileSwitchProcessInfo(std::size_t unique_id, std::string name) - : ProcessInfo(unique_id, std::move(name)) +ProfileSwitchTaskInfo::ProfileSwitchTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) { } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/seed_min_length_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/seed_min_length_task_generator.cpp similarity index 75% rename from tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/seed_min_length_process_generator.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/seed_min_length_task_generator.cpp index 8123e9294bd..a7b25f15d1b 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/seed_min_length_process_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/task_generators/seed_min_length_task_generator.cpp @@ -1,5 +1,5 @@ /** - * @file seed_length_process_generator.cpp + * @file seed_length_task_generator.cpp * @brief Process generator for processing the seed so it meets a minimum length. Planners like trajopt need * at least 10 states in the trajectory to perform velocity, accelleration and jerk smoothing. * @@ -30,38 +30,38 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include #include namespace tesseract_planning { -SeedMinLengthProcessGenerator::SeedMinLengthProcessGenerator(std::string name) : ProcessGenerator(std::move(name)) {} +SeedMinLengthTaskGenerator::SeedMinLengthTaskGenerator(std::string name) : TaskGenerator(std::move(name)) {} -SeedMinLengthProcessGenerator::SeedMinLengthProcessGenerator(long min_length, std::string name) - : ProcessGenerator(std::move(name)), min_length_(min_length) +SeedMinLengthTaskGenerator::SeedMinLengthTaskGenerator(long min_length, std::string name) + : TaskGenerator(std::move(name)), min_length_(min_length) { if (min_length_ <= 1) { - CONSOLE_BRIDGE_logWarn("SeedMinLengthProcessGenerator: The min length must be greater than 1, setting to default."); + CONSOLE_BRIDGE_logWarn("SeedMinLengthTaskGenerator: The min length must be greater than 1, setting to default."); min_length_ = 10; } } -int SeedMinLengthProcessGenerator::conditionalProcess(ProcessInput input, std::size_t unique_id) const +int SeedMinLengthTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const { if (input.isAborted()) return 0; - auto info = std::make_shared(unique_id, name_); + auto info = std::make_shared(unique_id, name_); info->return_value = 0; - input.addProcessInfo(info); + input.addTaskInfo(info); // Check that inputs are valid Instruction* input_results = input.getResults(); if (!isCompositeInstruction(*input_results)) { - CONSOLE_BRIDGE_logError("Input seed to SeedMinLengthProcessGenerator must be a composite instruction"); + CONSOLE_BRIDGE_logError("Input seed to SeedMinLengthTaskGenerator must be a composite instruction"); return 0; } @@ -88,15 +88,15 @@ int SeedMinLengthProcessGenerator::conditionalProcess(ProcessInput input, std::s return 1; } -void SeedMinLengthProcessGenerator::process(ProcessInput input, std::size_t unique_id) const +void SeedMinLengthTaskGenerator::process(TaskInput input, std::size_t unique_id) const { conditionalProcess(input, unique_id); } -void SeedMinLengthProcessGenerator::subdivide(CompositeInstruction& composite, - const CompositeInstruction& current_composite, - Instruction& start_instruction, - int subdivisions) const +void SeedMinLengthTaskGenerator::subdivide(CompositeInstruction& composite, + const CompositeInstruction& current_composite, + Instruction& start_instruction, + int subdivisions) const { for (const Instruction& i : current_composite) { @@ -144,8 +144,8 @@ void SeedMinLengthProcessGenerator::subdivide(CompositeInstruction& composite, } } } -SeedMinLengthProcessInfo::SeedMinLengthProcessInfo(std::size_t unique_id, std::string name) - : ProcessInfo(unique_id, std::move(name)) +SeedMinLengthTaskInfo::SeedMinLengthTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) { } } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp index 94639b3ba2b..5c6d2ad9fcc 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp @@ -31,11 +31,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -54,12 +54,10 @@ CartesianTaskflow::CartesianTaskflow(CartesianTaskflowParams params, std::string const std::string& CartesianTaskflow::getName() const { return name_; } -TaskflowContainer CartesianTaskflow::generateTaskflow(ProcessInput input, - TaskflowVoidFn done_cb, - TaskflowVoidFn error_cb) +TaskflowContainer CartesianTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -101,14 +99,14 @@ TaskflowContainer CartesianTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry()) interpolator->composite_profiles = input.profiles->getProfileEntry(); } - ProcessGenerator::UPtr interpolator_generator = std::make_unique(interpolator); + TaskGenerator::UPtr interpolator_generator = std::make_unique(interpolator); interpolator_generator->assignConditionalTask(input, interpolator_task); container.generators.push_back(std::move(interpolator_generator)); // Setup Seed Min Length Process Generator // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is // to short. - ProcessGenerator::UPtr seed_min_length_generator = std::make_unique(); + TaskGenerator::UPtr seed_min_length_generator = std::make_unique(); seed_min_length_generator->assignTask(input, seed_min_length_task); container.generators.push_back(std::move(seed_min_length_generator)); @@ -120,7 +118,7 @@ TaskflowContainer CartesianTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry>()) descartes_planner->plan_profiles = input.profiles->getProfileEntry>(); } - auto descartes_generator = std::make_unique(descartes_planner); + auto descartes_generator = std::make_unique(descartes_planner); descartes_generator->assignConditionalTask(input, descartes_task); container.generators.push_back(std::move(descartes_generator)); @@ -138,23 +136,23 @@ TaskflowContainer CartesianTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry()) trajopt_planner->solver_profiles = input.profiles->getProfileEntry(); } - ProcessGenerator::UPtr trajopt_generator = std::make_unique(trajopt_planner); + TaskGenerator::UPtr trajopt_generator = std::make_unique(trajopt_planner); trajopt_generator->assignConditionalTask(input, trajopt_task); container.generators.push_back(std::move(trajopt_generator)); - ProcessGenerator::UPtr contact_check_generator; + TaskGenerator::UPtr contact_check_generator; bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); if (has_contact_check) { if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); } - ProcessGenerator::UPtr time_parameterization_generator; + TaskGenerator::UPtr time_parameterization_generator; if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); + time_parameterization_generator = std::make_unique(); // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory if (has_contact_check && params_.enable_time_parameterization) @@ -196,12 +194,12 @@ TaskflowContainer CartesianTaskflow::generateTaskflow(ProcessInput input, return container; } -bool CartesianTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool CartesianTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // Check Input if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -209,7 +207,7 @@ bool CartesianTaskflow::checkProcessInput(const tesseract_planning::ProcessInput const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp index b9997bfab44..323d5d69e5f 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp @@ -30,10 +30,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -48,12 +48,10 @@ DescartesTaskflow::DescartesTaskflow(DescartesTaskflowParams params, std::string const std::string& DescartesTaskflow::getName() const { return name_; } -TaskflowContainer DescartesTaskflow::generateTaskflow(ProcessInput input, - TaskflowVoidFn done_cb, - TaskflowVoidFn error_cb) +TaskflowContainer DescartesTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -91,7 +89,7 @@ TaskflowContainer DescartesTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry()) interpolator->composite_profiles = input.profiles->getProfileEntry(); } - auto interpolator_generator = std::make_unique(interpolator); + auto interpolator_generator = std::make_unique(interpolator); interpolator_generator->assignConditionalTask(input, interpolator_task); container.generators.push_back(std::move(interpolator_generator)); @@ -103,23 +101,23 @@ TaskflowContainer DescartesTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry>()) descartes_planner->plan_profiles = input.profiles->getProfileEntry>(); } - auto descartes_generator = std::make_unique(descartes_planner); + auto descartes_generator = std::make_unique(descartes_planner); descartes_generator->assignConditionalTask(input, descartes_task); container.generators.push_back(std::move(descartes_generator)); - ProcessGenerator::UPtr contact_check_generator; + TaskGenerator::UPtr contact_check_generator; bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); if (has_contact_check) { if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); } - ProcessGenerator::UPtr time_parameterization_generator; + TaskGenerator::UPtr time_parameterization_generator; if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); + time_parameterization_generator = std::make_unique(); // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory if (has_contact_check && params_.enable_time_parameterization) @@ -161,12 +159,12 @@ TaskflowContainer DescartesTaskflow::generateTaskflow(ProcessInput input, return container; } -bool DescartesTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool DescartesTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // Check Input if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -174,7 +172,7 @@ bool DescartesTaskflow::checkProcessInput(const tesseract_planning::ProcessInput const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp index ae6040c6513..b4a3035deec 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp @@ -30,11 +30,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -53,12 +53,10 @@ FreespaceTaskflow::FreespaceTaskflow(FreespaceTaskflowParams params, std::string const std::string& FreespaceTaskflow::getName() const { return name_; } -TaskflowContainer FreespaceTaskflow::generateTaskflow(ProcessInput input, - TaskflowVoidFn done_cb, - TaskflowVoidFn error_cb) +TaskflowContainer FreespaceTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -104,11 +102,11 @@ TaskflowContainer FreespaceTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry()) interpolator->composite_profiles = input.profiles->getProfileEntry(); } - auto interpolator_generator = std::make_unique(interpolator); + auto interpolator_generator = std::make_unique(interpolator); interpolator_generator->assignConditionalTask(input, interpolator_task); container.generators.push_back(std::move(interpolator_generator)); - auto seed_min_length_generator = std::make_unique(); + auto seed_min_length_generator = std::make_unique(); seed_min_length_generator->assignTask(input, seed_min_length_task); container.generators.push_back(std::move(seed_min_length_generator)); @@ -119,7 +117,7 @@ TaskflowContainer FreespaceTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry()) ompl_planner->plan_profiles = input.profiles->getProfileEntry(); } - auto ompl_generator = std::make_unique(ompl_planner); + auto ompl_generator = std::make_unique(ompl_planner); ompl_generator->assignTask(input, ompl_task); container.generators.push_back(std::move(ompl_generator)); @@ -136,23 +134,23 @@ TaskflowContainer FreespaceTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry()) trajopt_planner->solver_profiles = input.profiles->getProfileEntry(); } - auto trajopt_generator = std::make_unique(trajopt_planner); + auto trajopt_generator = std::make_unique(trajopt_planner); trajopt_generator->assignConditionalTask(input, trajopt_task); container.generators.push_back(std::move(trajopt_generator)); - ProcessGenerator::UPtr contact_check_generator; + TaskGenerator::UPtr contact_check_generator; bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); if (has_contact_check) { if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); } - ProcessGenerator::UPtr time_parameterization_generator; + TaskGenerator::UPtr time_parameterization_generator; if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); + time_parameterization_generator = std::make_unique(); if (params_.type == FreespaceTaskflowType::TRAJOPT_FIRST) { @@ -174,7 +172,7 @@ TaskflowContainer FreespaceTaskflow::generateTaskflow(ProcessInput input, if (input.profiles->hasProfileEntry()) trajopt_planner2->solver_profiles = input.profiles->getProfileEntry(); } - ProcessGenerator::UPtr trajopt_generator2 = std::make_unique(trajopt_planner2); + TaskGenerator::UPtr trajopt_generator2 = std::make_unique(trajopt_planner2); trajopt_generator2->assignConditionalTask(input, trajopt_second_task); container.generators.push_back(std::move(trajopt_generator2)); @@ -267,12 +265,12 @@ TaskflowContainer FreespaceTaskflow::generateTaskflow(ProcessInput input, return container; } -bool FreespaceTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool FreespaceTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // Check Input if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -280,7 +278,7 @@ bool FreespaceTaskflow::checkProcessInput(const tesseract_planning::ProcessInput const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/graph_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/graph_taskflow.cpp index 7f0fa4ad8d8..564672c4fdf 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/graph_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/graph_taskflow.cpp @@ -38,7 +38,7 @@ GraphTaskflow::GraphTaskflow(std::string name) : name_(std::move(name)) {} const std::string& GraphTaskflow::getName() const { return name_; } -TaskflowContainer GraphTaskflow::generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) +TaskflowContainer GraphTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // Create Taskflow and Container TaskflowContainer container; @@ -180,7 +180,7 @@ TaskflowContainer GraphTaskflow::generateTaskflow(ProcessInput input, TaskflowVo return container; } -int GraphTaskflow::addNode(ProcessGenerator::UPtr process, NodeType process_type) +int GraphTaskflow::addNode(TaskGenerator::UPtr process, NodeType process_type) { Node pn; pn.process = std::move(process); diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp index b3b1b4a93ff..871304bf297 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp @@ -31,10 +31,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -49,10 +49,10 @@ OMPLTaskflow::OMPLTaskflow(OMPLTaskflowParams params, std::string name) : name_( const std::string& OMPLTaskflow::getName() const { return name_; } -TaskflowContainer OMPLTaskflow::generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) +TaskflowContainer OMPLTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -90,7 +90,7 @@ TaskflowContainer OMPLTaskflow::generateTaskflow(ProcessInput input, TaskflowVoi if (input.profiles->hasProfileEntry()) interpolator->composite_profiles = input.profiles->getProfileEntry(); } - auto interpolator_generator = std::make_unique(interpolator); + auto interpolator_generator = std::make_unique(interpolator); interpolator_generator->assignConditionalTask(input, interpolator_task); container.generators.push_back(std::move(interpolator_generator)); @@ -102,23 +102,23 @@ TaskflowContainer OMPLTaskflow::generateTaskflow(ProcessInput input, TaskflowVoi if (input.profiles->hasProfileEntry()) ompl_planner->plan_profiles = input.profiles->getProfileEntry(); } - auto ompl_generator = std::make_unique(ompl_planner); + auto ompl_generator = std::make_unique(ompl_planner); ompl_generator->assignConditionalTask(input, ompl_task); container.generators.push_back(std::move(ompl_generator)); - ProcessGenerator::UPtr contact_check_generator; + TaskGenerator::UPtr contact_check_generator; bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); if (has_contact_check) { if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); } - ProcessGenerator::UPtr time_parameterization_generator; + TaskGenerator::UPtr time_parameterization_generator; if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); + time_parameterization_generator = std::make_unique(); // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory if (has_contact_check && params_.enable_time_parameterization) @@ -160,12 +160,12 @@ TaskflowContainer OMPLTaskflow::generateTaskflow(ProcessInput input, TaskflowVoi return container; } -bool OMPLTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool OMPLTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // Check Input if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -173,7 +173,7 @@ bool OMPLTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& inp const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_dt_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_dt_taskflow.cpp index 142e8f4ebb0..d532303a457 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_dt_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_dt_taskflow.cpp @@ -54,12 +54,10 @@ RasterDTTaskflow::RasterDTTaskflow(TaskflowGenerator::UPtr freespace_taskflow_ge const std::string& RasterDTTaskflow::getName() const { return name_; } -TaskflowContainer RasterDTTaskflow::generateTaskflow(ProcessInput input, - TaskflowVoidFn done_cb, - TaskflowVoidFn error_cb) +TaskflowContainer RasterDTTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -96,7 +94,7 @@ TaskflowContainer RasterDTTaskflow::generateTaskflow(ProcessInput input, } start_instruction.cast()->setPlanType(PlanInstructionType::START); - ProcessInput raster_input = input[idx]; + TaskInput raster_input = input[idx]; raster_input.setStartInstruction(start_instruction); TaskflowContainer sub_container = raster_taskflow_generator_->generateTaskflow( raster_input, @@ -120,7 +118,7 @@ TaskflowContainer RasterDTTaskflow::generateTaskflow(ProcessInput input, // composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more // robust because planners could modify composite size, which is rare but does happen when using OMPL where it is // not possible to simplify the trajectory to the desired number of states. - ProcessInput transition_from_end_input = input[input_idx][0]; + TaskInput transition_from_end_input = input[input_idx][0]; transition_from_end_input.setStartInstruction(std::vector({ input_idx - 1 })); transition_from_end_input.setEndInstruction(std::vector({ input_idx + 1 })); TaskflowContainer sub_container1 = transition_taskflow_generator_->generateTaskflow( @@ -136,7 +134,7 @@ TaskflowContainer RasterDTTaskflow::generateTaskflow(ProcessInput input, transition_from_end_step.succeed(tasks[transition_idx]); transition_from_end_step.succeed(tasks[transition_idx + 1]); - ProcessInput transition_to_start_input = input[input_idx][1]; + TaskInput transition_to_start_input = input[input_idx][1]; transition_to_start_input.setStartInstruction(std::vector({ input_idx + 1 })); transition_to_start_input.setEndInstruction(std::vector({ input_idx - 1 })); TaskflowContainer sub_container2 = transition_taskflow_generator_->generateTaskflow( @@ -156,7 +154,7 @@ TaskflowContainer RasterDTTaskflow::generateTaskflow(ProcessInput input, } // Plan from_start - preceded by the first raster - ProcessInput from_start_input = input[0]; + TaskInput from_start_input = input[0]; from_start_input.setStartInstruction( input.getInstruction()->cast_const()->getStartInstruction()); from_start_input.setEndInstruction(std::vector({ 1 })); @@ -170,7 +168,7 @@ TaskflowContainer RasterDTTaskflow::generateTaskflow(ProcessInput input, tasks[0].precede(from_start); // Plan to_end - preceded by the last raster - ProcessInput to_end_input = input[input.size() - 1]; + TaskInput to_end_input = input[input.size() - 1]; to_end_input.setStartInstruction(std::vector({ input.size() - 2 })); TaskflowContainer sub_container2 = freespace_taskflow_generator_->generateTaskflow( to_end_input, @@ -184,14 +182,14 @@ TaskflowContainer RasterDTTaskflow::generateTaskflow(ProcessInput input, return container; } -bool RasterDTTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool RasterDTTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // ------------- // Check Input // ------------- if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -199,7 +197,7 @@ bool RasterDTTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } const auto* composite = input_instruction->cast_const(); @@ -207,14 +205,14 @@ bool RasterDTTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& // Check that it has a start instruction if (!composite->hasStartInstruction() && isNullInstruction(input.getStartInstruction())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should have a start instruction"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should have a start instruction"); return false; } // Check from_start if (!isCompositeInstruction(composite->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: from_start should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: from_start should be a composite"); return false; } @@ -224,7 +222,7 @@ bool RasterDTTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& // Both rasters and transitions should be a composite if (!isCompositeInstruction(composite->at(index))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Both rasters and transitions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Both rasters and transitions should be a composite"); return false; } @@ -249,19 +247,19 @@ bool RasterDTTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& if (!isCompositeInstruction(step->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: transition from end should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: transition from end should be a composite"); return false; } if (!isCompositeInstruction(step->at(1))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: transition to start should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: transition to start should be a composite"); return false; } } else { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: transition should be a composite of size 2"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: transition should be a composite of size 2"); return false; } } @@ -269,7 +267,7 @@ bool RasterDTTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& // Check to_end if (!isCompositeInstruction(composite->back())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: to_end should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: to_end should be a composite"); return false; }; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_global_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_global_taskflow.cpp index 949669cf903..2e9cce8424e 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_global_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_global_taskflow.cpp @@ -56,12 +56,12 @@ RasterGlobalTaskflow::RasterGlobalTaskflow(TaskflowGenerator::UPtr global_taskfl const std::string& RasterGlobalTaskflow::getName() const { return name_; } -TaskflowContainer RasterGlobalTaskflow::generateTaskflow(ProcessInput input, +TaskflowContainer RasterGlobalTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -88,7 +88,7 @@ TaskflowContainer RasterGlobalTaskflow::generateTaskflow(ProcessInput input, std::size_t raster_idx = 0; for (std::size_t idx = 1; idx < input.size() - 1; idx += 2) { - ProcessInput raster_input = input[idx]; + TaskInput raster_input = input[idx]; raster_input.setStartInstruction(std::vector({ idx - 1 })); raster_input.setEndInstruction(std::vector({ idx + 1 })); TaskflowContainer sub_container = raster_taskflow_generator_->generateTaskflow( @@ -114,7 +114,7 @@ TaskflowContainer RasterGlobalTaskflow::generateTaskflow(ProcessInput input, // composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more // robust because planners could modify composite size, which is rare but does happen when using OMPL where it is // not possible to simplify the trajectory to the desired number of states. - ProcessInput transition_input = input[input_idx]; + TaskInput transition_input = input[input_idx]; transition_input.setStartInstruction(std::vector({ input_idx - 1 })); transition_input.setEndInstruction(std::vector({ input_idx + 1 })); TaskflowContainer sub_container = transition_taskflow_generator_->generateTaskflow( @@ -135,7 +135,7 @@ TaskflowContainer RasterGlobalTaskflow::generateTaskflow(ProcessInput input, } // Plan from_start - preceded by the first raster - ProcessInput from_start_input = input[0]; + TaskInput from_start_input = input[0]; from_start_input.setStartInstruction(input_instruction->cast_const()->getStartInstruction()); from_start_input.setEndInstruction(std::vector({ 1 })); @@ -150,7 +150,7 @@ TaskflowContainer RasterGlobalTaskflow::generateTaskflow(ProcessInput input, tasks[0].precede(from_start); // Plan to_end - preceded by the last raster - ProcessInput to_end_input = input[input.size() - 1]; + TaskInput to_end_input = input[input.size() - 1]; to_end_input.setStartInstruction(std::vector({ input.size() - 2 })); TaskflowContainer sub_container2 = freespace_taskflow_generator_->generateTaskflow( to_end_input, @@ -165,7 +165,7 @@ TaskflowContainer RasterGlobalTaskflow::generateTaskflow(ProcessInput input, return container; } -void RasterGlobalTaskflow::globalPostProcess(ProcessInput input) +void RasterGlobalTaskflow::globalPostProcess(TaskInput input) { if (input.isAborted()) return; @@ -186,14 +186,14 @@ void RasterGlobalTaskflow::globalPostProcess(ProcessInput input) } } -bool RasterGlobalTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool RasterGlobalTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // ------------- // Check Input // ------------- if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -201,7 +201,7 @@ bool RasterGlobalTaskflow::checkProcessInput(const tesseract_planning::ProcessIn const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } const auto* composite = input_instruction->cast_const(); @@ -209,14 +209,14 @@ bool RasterGlobalTaskflow::checkProcessInput(const tesseract_planning::ProcessIn // Check that it has a start instruction if (!composite->hasStartInstruction() && isNullInstruction(input.getStartInstruction())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should have a start instruction"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should have a start instruction"); return false; } // Check from_start if (!isCompositeInstruction(composite->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: from_start should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: from_start should be a composite"); return false; } @@ -226,7 +226,7 @@ bool RasterGlobalTaskflow::checkProcessInput(const tesseract_planning::ProcessIn // Both rasters and transitions should be a composite if (!isCompositeInstruction(composite->at(index))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Both rasters and transitions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Both rasters and transitions should be a composite"); return false; } } @@ -234,7 +234,7 @@ bool RasterGlobalTaskflow::checkProcessInput(const tesseract_planning::ProcessIn // Check to_end if (!isCompositeInstruction(composite->back())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: to_end should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: to_end should be a composite"); return false; }; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_global_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_global_taskflow.cpp index d3a82a33471..7275fd19be9 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_global_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_global_taskflow.cpp @@ -54,12 +54,12 @@ RasterOnlyGlobalTaskflow::RasterOnlyGlobalTaskflow(TaskflowGenerator::UPtr globa const std::string& RasterOnlyGlobalTaskflow::getName() const { return name_; } -TaskflowContainer RasterOnlyGlobalTaskflow::generateTaskflow(ProcessInput input, +TaskflowContainer RasterOnlyGlobalTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -86,7 +86,7 @@ TaskflowContainer RasterOnlyGlobalTaskflow::generateTaskflow(ProcessInput input, std::size_t raster_idx = 0; for (std::size_t idx = 0; idx < input.size(); idx += 2) { - ProcessInput raster_input = input[idx]; + TaskInput raster_input = input[idx]; if (idx == 0) raster_input.setStartInstruction(input_instruction->cast_const()->getStartInstruction()); else @@ -118,7 +118,7 @@ TaskflowContainer RasterOnlyGlobalTaskflow::generateTaskflow(ProcessInput input, // composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more // robust because planners could modify composite size, which is rare but does happen when using OMPL where it is // not possible to simplify the trajectory to the desired number of states. - ProcessInput transition_input = input[input_idx]; + TaskInput transition_input = input[input_idx]; transition_input.setStartInstruction(std::vector({ input_idx - 1 })); transition_input.setEndInstruction(std::vector({ input_idx + 1 })); TaskflowContainer sub_container = transition_taskflow_generator_->generateTaskflow( @@ -141,7 +141,7 @@ TaskflowContainer RasterOnlyGlobalTaskflow::generateTaskflow(ProcessInput input, return container; } -void RasterOnlyGlobalTaskflow::globalPostProcess(ProcessInput input) +void RasterOnlyGlobalTaskflow::globalPostProcess(TaskInput input) { if (input.isAborted()) return; @@ -162,14 +162,14 @@ void RasterOnlyGlobalTaskflow::globalPostProcess(ProcessInput input) } } -bool RasterOnlyGlobalTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool RasterOnlyGlobalTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // ------------- // Check Input // ------------- if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -177,7 +177,7 @@ bool RasterOnlyGlobalTaskflow::checkProcessInput(const tesseract_planning::Proce const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } const auto* composite = input_instruction->cast_const(); @@ -185,7 +185,7 @@ bool RasterOnlyGlobalTaskflow::checkProcessInput(const tesseract_planning::Proce // Check that it has a start instruction if (!composite->hasStartInstruction() && isNullInstruction(input.getStartInstruction())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should have a start instruction"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should have a start instruction"); return false; } @@ -195,7 +195,7 @@ bool RasterOnlyGlobalTaskflow::checkProcessInput(const tesseract_planning::Proce // Both rasters and transitions should be a composite if (!isCompositeInstruction(composite->at(index))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Both rasters and transitions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Both rasters and transitions should be a composite"); return false; } } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_taskflow.cpp index c53c97503e5..b008f4ad662 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_only_taskflow.cpp @@ -52,12 +52,10 @@ RasterOnlyTaskflow::RasterOnlyTaskflow(TaskflowGenerator::UPtr transition_taskfl const std::string& RasterOnlyTaskflow::getName() const { return name_; } -TaskflowContainer RasterOnlyTaskflow::generateTaskflow(ProcessInput input, - TaskflowVoidFn done_cb, - TaskflowVoidFn error_cb) +TaskflowContainer RasterOnlyTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -82,7 +80,7 @@ TaskflowContainer RasterOnlyTaskflow::generateTaskflow(ProcessInput input, } else { - ProcessInput pre_input = input[idx - 1]; + TaskInput pre_input = input[idx - 1]; const Instruction* pre_input_instruction = pre_input.getInstruction(); assert(isCompositeInstruction(*pre_input_instruction)); const auto* tci = pre_input_instruction->cast_const(); @@ -92,7 +90,7 @@ TaskflowContainer RasterOnlyTaskflow::generateTaskflow(ProcessInput input, } start_instruction.cast()->setPlanType(PlanInstructionType::START); - ProcessInput raster_input = input[idx]; + TaskInput raster_input = input[idx]; raster_input.setStartInstruction(start_instruction); TaskflowContainer sub_container = raster_taskflow_generator_->generateTaskflow( raster_input, @@ -117,7 +115,7 @@ TaskflowContainer RasterOnlyTaskflow::generateTaskflow(ProcessInput input, // composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more // robust because planners could modify composite size, which is rare but does happen when using OMPL where it is // not possible to simplify the trajectory to the desired number of states. - ProcessInput transition_input = input[input_idx]; + TaskInput transition_input = input[input_idx]; transition_input.setStartInstruction(std::vector({ input_idx - 1 })); transition_input.setEndInstruction(std::vector({ input_idx + 1 })); TaskflowContainer sub_container = transition_taskflow_generator_->generateTaskflow( @@ -140,14 +138,14 @@ TaskflowContainer RasterOnlyTaskflow::generateTaskflow(ProcessInput input, return container; } -bool RasterOnlyTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool RasterOnlyTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // ------------- // Check Input // ------------- if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -155,7 +153,7 @@ bool RasterOnlyTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } const auto* composite = input_instruction->cast_const(); @@ -163,7 +161,7 @@ bool RasterOnlyTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu // Check that it has a start instruction if (!composite->hasStartInstruction() && isNullInstruction(input.getStartInstruction())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should have a start instruction"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should have a start instruction"); return false; } @@ -173,7 +171,7 @@ bool RasterOnlyTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu // Both rasters and transitions should be a composite if (!isCompositeInstruction(composite->at(index))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Both rasters and transitions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Both rasters and transitions should be a composite"); return false; } } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_taskflow.cpp index b561ae477aa..101921240ec 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_taskflow.cpp @@ -54,10 +54,10 @@ RasterTaskflow::RasterTaskflow(TaskflowGenerator::UPtr freespace_taskflow_genera const std::string& RasterTaskflow::getName() const { return name_; } -TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) +TaskflowContainer RasterTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -77,7 +77,7 @@ TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowV Instruction start_instruction = NullInstruction(); if (idx == 1) { - ProcessInput from_start_input = input[0]; + TaskInput from_start_input = input[0]; const Instruction* from_start_input_instruction = from_start_input.getInstruction(); assert(isCompositeInstruction(*from_start_input_instruction)); const auto* ci = from_start_input_instruction->cast_const(); @@ -87,7 +87,7 @@ TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowV } else { - ProcessInput pre_input = input[idx - 1]; + TaskInput pre_input = input[idx - 1]; const Instruction* pre_input_instruction = pre_input.getInstruction(); assert(isCompositeInstruction(*pre_input_instruction)); const auto* tci = pre_input_instruction->cast_const(); @@ -97,7 +97,7 @@ TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowV } start_instruction.cast()->setPlanType(PlanInstructionType::START); - ProcessInput raster_input = input[idx]; + TaskInput raster_input = input[idx]; raster_input.setStartInstruction(start_instruction); TaskflowContainer sub_container = raster_taskflow_generator_->generateTaskflow( raster_input, @@ -122,7 +122,7 @@ TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowV // composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more // robust because planners could modify composite size, which is rare but does happen when using OMPL where it is // not possible to simplify the trajectory to the desired number of states. - ProcessInput transition_input = input[input_idx]; + TaskInput transition_input = input[input_idx]; transition_input.setStartInstruction(std::vector({ input_idx - 1 })); transition_input.setEndInstruction(std::vector({ input_idx + 1 })); TaskflowContainer sub_container = transition_taskflow_generator_->generateTaskflow( @@ -143,7 +143,7 @@ TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowV } // Plan from_start - preceded by the first raster - ProcessInput from_start_input = input[0]; + TaskInput from_start_input = input[0]; from_start_input.setStartInstruction(input_instruction->cast_const()->getStartInstruction()); from_start_input.setEndInstruction(std::vector({ 1 })); TaskflowContainer sub_container1 = freespace_taskflow_generator_->generateTaskflow( @@ -157,7 +157,7 @@ TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowV tasks[0].precede(from_start); // Plan to_end - preceded by the last raster - ProcessInput to_end_input = input[input.size() - 1]; + TaskInput to_end_input = input[input.size() - 1]; to_end_input.setStartInstruction(std::vector({ input.size() - 2 })); TaskflowContainer sub_container2 = freespace_taskflow_generator_->generateTaskflow( to_end_input, @@ -172,14 +172,14 @@ TaskflowContainer RasterTaskflow::generateTaskflow(ProcessInput input, TaskflowV return container; } -bool RasterTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool RasterTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // ------------- // Check Input // ------------- if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -187,7 +187,7 @@ bool RasterTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& i const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } const auto* composite = input_instruction->cast_const(); @@ -195,14 +195,14 @@ bool RasterTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& i // Check that it has a start instruction if (!composite->hasStartInstruction() && isNullInstruction(input.getStartInstruction())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should have a start instruction"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should have a start instruction"); return false; } // Check from_start if (!isCompositeInstruction(composite->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: from_start should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: from_start should be a composite"); return false; } @@ -212,7 +212,7 @@ bool RasterTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& i // Both rasters and transitions should be a composite if (!isCompositeInstruction(composite->at(index))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Both rasters and transitions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Both rasters and transitions should be a composite"); return false; } } @@ -220,7 +220,7 @@ bool RasterTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& i // Check to_end if (!isCompositeInstruction(composite->back())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: to_end should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: to_end should be a composite"); return false; }; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_dt_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_dt_taskflow.cpp index 4d65cef4adc..d9bf5b682a0 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_dt_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_dt_taskflow.cpp @@ -54,12 +54,12 @@ RasterWAADDTTaskflow::RasterWAADDTTaskflow(TaskflowGenerator::UPtr freespace_tas const std::string& RasterWAADDTTaskflow::getName() const { return name_; } -TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, +TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -81,19 +81,19 @@ TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, assert(ali != nullptr); // Create the process taskflow - ProcessInput process_input = input[idx][1]; - process_input.setStartInstruction(*ali); + TaskInput task_input = input[idx][1]; + task_input.setStartInstruction(*ali); TaskflowContainer sub_container1 = raster_taskflow_generator_->generateTaskflow( - process_input, - [=]() { successTask(input, name_, process_input.getInstruction()->getDescription(), done_cb); }, - [=]() { failureTask(input, name_, process_input.getInstruction()->getDescription(), error_cb); }); + task_input, + [=]() { successTask(input, name_, task_input.getInstruction()->getDescription(), done_cb); }, + [=]() { failureTask(input, name_, task_input.getInstruction()->getDescription(), error_cb); }); auto process_step = container.taskflow->composed_of(*(sub_container1.taskflow)).name("raster_" + std::to_string(raster_idx + 1)); container.containers.push_back(std::move(sub_container1)); // Create Departure Taskflow - ProcessInput departure_input = input[idx][2]; + TaskInput departure_input = input[idx][2]; departure_input.setStartInstruction(std::vector({ idx, 1 })); TaskflowContainer sub_container2 = raster_taskflow_generator_->generateTaskflow( departure_input, @@ -125,7 +125,7 @@ TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, // Create the departure taskflow start_instruction.cast()->setPlanType(PlanInstructionType::START); - ProcessInput approach_input = input[idx][0]; + TaskInput approach_input = input[idx][0]; approach_input.setStartInstruction(start_instruction); approach_input.setEndInstruction(std::vector({ idx, 1 })); TaskflowContainer sub_container0 = raster_taskflow_generator_->generateTaskflow( @@ -155,7 +155,7 @@ TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, // composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more // robust because planners could modify composite size, which is rare but does happen when using OMPL where it is // not possible to simplify the trajectory to the desired number of states. - ProcessInput transition_from_end_input = input[input_idx][0]; + TaskInput transition_from_end_input = input[input_idx][0]; transition_from_end_input.setStartInstruction(std::vector({ input_idx - 1, 2 })); transition_from_end_input.setEndInstruction(std::vector({ input_idx + 1, 0 })); TaskflowContainer sub_container1 = transition_taskflow_generator_->generateTaskflow( @@ -171,7 +171,7 @@ TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, transition_from_end_step.succeed(raster_tasks[transition_idx][2]); transition_from_end_step.succeed(raster_tasks[transition_idx + 1][0]); - ProcessInput transition_to_start_input = input[input_idx][1]; + TaskInput transition_to_start_input = input[input_idx][1]; transition_to_start_input.setStartInstruction(std::vector({ input_idx + 1, 2 })); transition_to_start_input.setEndInstruction(std::vector({ input_idx - 1, 0 })); TaskflowContainer sub_container2 = transition_taskflow_generator_->generateTaskflow( @@ -190,7 +190,7 @@ TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, } // Plan from_start - preceded by the first raster - ProcessInput from_start_input = input[0]; + TaskInput from_start_input = input[0]; from_start_input.setStartInstruction( input.getInstruction()->cast_const()->getStartInstruction()); from_start_input.setEndInstruction(std::vector({ 1, 0 })); @@ -204,7 +204,7 @@ TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, raster_tasks[0][0].precede(from_start); // Plan to_end - preceded by the last raster - ProcessInput to_end_input = input[input.size() - 1]; + TaskInput to_end_input = input[input.size() - 1]; to_end_input.setStartInstruction(std::vector({ input.size() - 2, 2 })); TaskflowContainer sub_container2 = freespace_taskflow_generator_->generateTaskflow( to_end_input, @@ -218,14 +218,14 @@ TaskflowContainer RasterWAADDTTaskflow::generateTaskflow(ProcessInput input, return container; } -bool RasterWAADDTTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool RasterWAADDTTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // ------------- // Check Input // ------------- if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -233,7 +233,7 @@ bool RasterWAADDTTaskflow::checkProcessInput(const tesseract_planning::ProcessIn const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } const auto* composite = input_instruction->cast_const(); @@ -241,14 +241,14 @@ bool RasterWAADDTTaskflow::checkProcessInput(const tesseract_planning::ProcessIn // Check that it has a start instruction if (!composite->hasStartInstruction() && isNullInstruction(input.getStartInstruction())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should have a start instruction"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should have a start instruction"); return false; } // Check from_start if (!isCompositeInstruction(composite->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: from_start should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: from_start should be a composite"); return false; } @@ -258,7 +258,7 @@ bool RasterWAADDTTaskflow::checkProcessInput(const tesseract_planning::ProcessIn // Both rasters and transitions should be a composite if (!isCompositeInstruction(composite->at(index))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Both rasters and transitions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Both rasters and transitions should be a composite"); return false; } @@ -271,27 +271,27 @@ bool RasterWAADDTTaskflow::checkProcessInput(const tesseract_planning::ProcessIn // Raster must have three composites approach, raster and departure if (step->size() != 3) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Rasters must have three element approach, raster and departure"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Rasters must have three element approach, raster and departure"); return false; } // The approach should be a composite if (!isCompositeInstruction(step->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: The raster approach should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: The raster approach should be a composite"); return false; } // The process should be a composite if (!isCompositeInstruction(step->at(1))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: The process should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: The process should be a composite"); return false; } // The departure should be a composite if (!isCompositeInstruction(step->at(2))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: The departure should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: The departure should be a composite"); return false; } } @@ -312,19 +312,19 @@ bool RasterWAADDTTaskflow::checkProcessInput(const tesseract_planning::ProcessIn if (!isCompositeInstruction(step->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: transition from end should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: transition from end should be a composite"); return false; } if (!isCompositeInstruction(step->at(1))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: transition to start should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: transition to start should be a composite"); return false; } } else { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: transition should be a composite of size 2"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: transition should be a composite of size 2"); return false; } } @@ -333,7 +333,7 @@ bool RasterWAADDTTaskflow::checkProcessInput(const tesseract_planning::ProcessIn // Check to_end if (!isCompositeInstruction(composite->back())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: to_end should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: to_end should be a composite"); return false; }; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_taskflow.cpp index 4e5d5280321..6c64bccd5f8 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/raster_waad_taskflow.cpp @@ -54,12 +54,10 @@ RasterWAADTaskflow::RasterWAADTaskflow(TaskflowGenerator::UPtr freespace_taskflo const std::string& RasterWAADTaskflow::getName() const { return name_; } -TaskflowContainer RasterWAADTaskflow::generateTaskflow(ProcessInput input, - TaskflowVoidFn done_cb, - TaskflowVoidFn error_cb) +TaskflowContainer RasterWAADTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -81,19 +79,19 @@ TaskflowContainer RasterWAADTaskflow::generateTaskflow(ProcessInput input, assert(ali != nullptr); // Create the process taskflow - ProcessInput process_input = input[idx][1]; - process_input.setStartInstruction(*ali); + TaskInput task_input = input[idx][1]; + task_input.setStartInstruction(*ali); TaskflowContainer sub_container1 = raster_taskflow_generator_->generateTaskflow( - process_input, - [=]() { successTask(input, name_, process_input.getInstruction()->getDescription(), done_cb); }, - [=]() { failureTask(input, name_, process_input.getInstruction()->getDescription(), error_cb); }); + task_input, + [=]() { successTask(input, name_, task_input.getInstruction()->getDescription(), done_cb); }, + [=]() { failureTask(input, name_, task_input.getInstruction()->getDescription(), error_cb); }); auto process_step = container.taskflow->composed_of(*(sub_container1.taskflow)).name("raster_" + std::to_string(raster_idx + 1)); container.containers.push_back(std::move(sub_container1)); // Create Departure Taskflow - ProcessInput departure_input = input[idx][2]; + TaskInput departure_input = input[idx][2]; departure_input.setStartInstruction(std::vector({ idx, 1 })); TaskflowContainer sub_container2 = raster_taskflow_generator_->generateTaskflow( departure_input, @@ -125,7 +123,7 @@ TaskflowContainer RasterWAADTaskflow::generateTaskflow(ProcessInput input, // Create the departure taskflow start_instruction.cast()->setPlanType(PlanInstructionType::START); - ProcessInput approach_input = input[idx][0]; + TaskInput approach_input = input[idx][0]; approach_input.setStartInstruction(start_instruction); approach_input.setEndInstruction(std::vector({ idx, 1 })); TaskflowContainer sub_container0 = raster_taskflow_generator_->generateTaskflow( @@ -155,7 +153,7 @@ TaskflowContainer RasterWAADTaskflow::generateTaskflow(ProcessInput input, // composite and let the generateTaskflow extract the start and end waypoint from the composite. This is also more // robust because planners could modify composite size, which is rare but does happen when using OMPL where it is // not possible to simplify the trajectory to the desired number of states. - ProcessInput transition_input = input[input_idx]; + TaskInput transition_input = input[input_idx]; transition_input.setStartInstruction(std::vector({ input_idx - 1, 2 })); transition_input.setEndInstruction(std::vector({ input_idx + 1, 0 })); TaskflowContainer sub_container = transition_taskflow_generator_->generateTaskflow( @@ -174,7 +172,7 @@ TaskflowContainer RasterWAADTaskflow::generateTaskflow(ProcessInput input, } // Plan from_start - preceded by the first raster - ProcessInput from_start_input = input[0]; + TaskInput from_start_input = input[0]; from_start_input.setStartInstruction( input.getInstruction()->cast_const()->getStartInstruction()); from_start_input.setEndInstruction(std::vector({ 1, 0 })); @@ -188,7 +186,7 @@ TaskflowContainer RasterWAADTaskflow::generateTaskflow(ProcessInput input, raster_tasks[0][0].precede(from_start); // Plan to_end - preceded by the last raster - ProcessInput to_end_input = input[input.size() - 1]; + TaskInput to_end_input = input[input.size() - 1]; to_end_input.setStartInstruction(std::vector({ input.size() - 2, 2 })); TaskflowContainer sub_container2 = freespace_taskflow_generator_->generateTaskflow( to_end_input, @@ -202,14 +200,14 @@ TaskflowContainer RasterWAADTaskflow::generateTaskflow(ProcessInput input, return container; } -bool RasterWAADTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool RasterWAADTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // ------------- // Check Input // ------------- if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -217,7 +215,7 @@ bool RasterWAADTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } const auto* composite = input_instruction->cast_const(); @@ -225,14 +223,14 @@ bool RasterWAADTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu // Check that it has a start instruction if (!composite->hasStartInstruction() && isNullInstruction(input.getStartInstruction())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should have a start instruction"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should have a start instruction"); return false; } // Check from_start if (!isCompositeInstruction(composite->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: from_start should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: from_start should be a composite"); return false; } @@ -242,7 +240,7 @@ bool RasterWAADTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu // Both rasters and transitions should be a composite if (!isCompositeInstruction(composite->at(index))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Both rasters and transitions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Both rasters and transitions should be a composite"); return false; } @@ -255,27 +253,27 @@ bool RasterWAADTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu // Raster must have three composites approach, raster and departure if (step->size() != 3) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: Rasters must have three element approach, raster and departure"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: Rasters must have three element approach, raster and departure"); return false; } // The approach should be a composite if (!isCompositeInstruction(step->at(0))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: The raster approach should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: The raster approach should be a composite"); return false; } // The process should be a composite if (!isCompositeInstruction(step->at(1))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: The process should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: The process should be a composite"); return false; } // The departure should be a composite if (!isCompositeInstruction(step->at(2))) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: The departure should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: The departure should be a composite"); return false; } } @@ -284,7 +282,7 @@ bool RasterWAADTaskflow::checkProcessInput(const tesseract_planning::ProcessInpu // Check to_end if (!isCompositeInstruction(composite->back())) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: to_end should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: to_end should be a composite"); return false; }; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp index f8474e9e6df..9253a0d6d63 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp @@ -31,11 +31,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -50,10 +50,10 @@ TrajOptTaskflow::TrajOptTaskflow(TrajOptTaskflowParams params, std::string name) const std::string& TrajOptTaskflow::getName() const { return name_; } -TaskflowContainer TrajOptTaskflow::generateTaskflow(ProcessInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) +TaskflowContainer TrajOptTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) { // This should make all of the isComposite checks so that you can safely cast below - if (!checkProcessInput(input)) + if (!checkTaskInput(input)) { CONSOLE_BRIDGE_logError("Invalid Process Input"); throw std::runtime_error("Invalid Process Input"); @@ -93,14 +93,14 @@ TaskflowContainer TrajOptTaskflow::generateTaskflow(ProcessInput input, Taskflow if (input.profiles->hasProfileEntry()) interpolator->composite_profiles = input.profiles->getProfileEntry(); } - ProcessGenerator::UPtr interpolator_generator = std::make_unique(interpolator); + TaskGenerator::UPtr interpolator_generator = std::make_unique(interpolator); interpolator_generator->assignConditionalTask(input, interpolator_task); container.generators.push_back(std::move(interpolator_generator)); // Setup Seed Min Length Process Generator // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is // to short. - ProcessGenerator::UPtr seed_min_length_generator = std::make_unique(); + TaskGenerator::UPtr seed_min_length_generator = std::make_unique(); seed_min_length_generator->assignTask(input, seed_min_length_task); container.generators.push_back(std::move(seed_min_length_generator)); @@ -118,23 +118,23 @@ TaskflowContainer TrajOptTaskflow::generateTaskflow(ProcessInput input, Taskflow if (input.profiles->hasProfileEntry()) trajopt_planner->solver_profiles = input.profiles->getProfileEntry(); } - ProcessGenerator::UPtr trajopt_generator = std::make_unique(trajopt_planner); + TaskGenerator::UPtr trajopt_generator = std::make_unique(trajopt_planner); trajopt_generator->assignConditionalTask(input, trajopt_task); container.generators.push_back(std::move(trajopt_generator)); - ProcessGenerator::UPtr contact_check_generator; + TaskGenerator::UPtr contact_check_generator; bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); if (has_contact_check) { if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); + contact_check_generator = std::make_unique(); } - ProcessGenerator::UPtr time_parameterization_generator; + TaskGenerator::UPtr time_parameterization_generator; if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); + time_parameterization_generator = std::make_unique(); // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory if (has_contact_check && params_.enable_time_parameterization) @@ -176,12 +176,12 @@ TaskflowContainer TrajOptTaskflow::generateTaskflow(ProcessInput input, Taskflow return container; } -bool TrajOptTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& input) const +bool TrajOptTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) const { // Check Input if (!input.env) { - CONSOLE_BRIDGE_logError("ProcessInput env is a nullptr"); + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); return false; } @@ -189,7 +189,7 @@ bool TrajOptTaskflow::checkProcessInput(const tesseract_planning::ProcessInput& const Instruction* input_instruction = input.getInstruction(); if (!isCompositeInstruction(*input_instruction)) { - CONSOLE_BRIDGE_logError("ProcessInput Invalid: input.instructions should be a composite"); + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); return false; } diff --git a/tesseract/tesseract_planning/tesseract_process_managers/test/CMakeLists.txt b/tesseract/tesseract_planning/tesseract_process_managers/test/CMakeLists.txt index 9b10ac6b8cc..4278f1a6526 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/test/CMakeLists.txt +++ b/tesseract/tesseract_planning/tesseract_process_managers/test/CMakeLists.txt @@ -44,13 +44,13 @@ add_gtest_discover_tests(${PROJECT_NAME}_unit) add_dependencies(${PROJECT_NAME}_unit ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_unit) -add_executable(${PROJECT_NAME}_fix_state_collision_process_generator_unit fix_state_collision_process_generator_unit.cpp) -target_link_libraries(${PROJECT_NAME}_fix_state_collision_process_generator_unit PRIVATE GTest::GTest GTest::Main tesseract::tesseract_support tesseract::tesseract_environment_ofkt ${PROJECT_NAME}) -target_include_directories(${PROJECT_NAME}_fix_state_collision_process_generator_unit PUBLIC +add_executable(${PROJECT_NAME}_fix_state_collision_task_generator_unit fix_state_collision_task_generator_unit.cpp) +target_link_libraries(${PROJECT_NAME}_fix_state_collision_task_generator_unit PRIVATE GTest::GTest GTest::Main tesseract::tesseract_support tesseract::tesseract_environment_ofkt ${PROJECT_NAME}) +target_include_directories(${PROJECT_NAME}_fix_state_collision_task_generator_unit PUBLIC "$") -target_compile_options(${PROJECT_NAME}_fix_state_collision_process_generator_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS}) -target_clang_tidy(${PROJECT_NAME}_fix_state_collision_process_generator_unit ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_fix_state_collision_process_generator_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage(${PROJECT_NAME}_fix_state_collision_process_generator_unit ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_fix_state_collision_process_generator_unit) -add_dependencies(run_tests ${PROJECT_NAME}_fix_state_collision_process_generator_unit) +target_compile_options(${PROJECT_NAME}_fix_state_collision_task_generator_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS}) +target_clang_tidy(${PROJECT_NAME}_fix_state_collision_task_generator_unit ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_fix_state_collision_task_generator_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) +target_code_coverage(${PROJECT_NAME}_fix_state_collision_task_generator_unit ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) +add_gtest_discover_tests(${PROJECT_NAME}_fix_state_collision_task_generator_unit) +add_dependencies(run_tests ${PROJECT_NAME}_fix_state_collision_task_generator_unit) diff --git a/tesseract/tesseract_planning/tesseract_process_managers/test/fix_state_collision_process_generator_unit.cpp b/tesseract/tesseract_planning/tesseract_process_managers/test/fix_state_collision_task_generator_unit.cpp similarity index 89% rename from tesseract/tesseract_planning/tesseract_process_managers/test/fix_state_collision_process_generator_unit.cpp rename to tesseract/tesseract_planning/tesseract_process_managers/test/fix_state_collision_task_generator_unit.cpp index d1062d59d9c..45dd62f0327 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/test/fix_state_collision_process_generator_unit.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/test/fix_state_collision_task_generator_unit.cpp @@ -5,7 +5,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include #include #include @@ -45,7 +45,7 @@ std::string locateResource(const std::string& url) return mod_url; } -class FixStateCollisionProcessGeneratorUnit : public ::testing::Test +class FixStateCollisionTaskGeneratorUnit : public ::testing::Test { protected: Environment::Ptr env_; @@ -66,12 +66,12 @@ class FixStateCollisionProcessGeneratorUnit : public ::testing::Test } }; -TEST_F(FixStateCollisionProcessGeneratorUnit, StateInCollisionTest) +TEST_F(FixStateCollisionTaskGeneratorUnit, StateInCollisionTest) { CompositeInstruction program = freespaceExampleProgramABB(); const Instruction program_instruction{ program }; Instruction seed = generateSkeletonSeed(program); - ProcessInput input(env_, &program_instruction, manip_, &seed, false, nullptr); + TaskInput input(env_, &program_instruction, manip_, &seed, false, nullptr); FixStateCollisionProfile profile; @@ -105,12 +105,12 @@ TEST_F(FixStateCollisionProcessGeneratorUnit, StateInCollisionTest) EXPECT_TRUE(contacts.empty()); } -TEST_F(FixStateCollisionProcessGeneratorUnit, WaypointInCollisionTest) +TEST_F(FixStateCollisionTaskGeneratorUnit, WaypointInCollisionTest) { CompositeInstruction program = freespaceExampleProgramABB(); const Instruction program_instruction{ program }; Instruction seed = generateSkeletonSeed(program); - ProcessInput input(env_, &program_instruction, manip_, &seed, false, nullptr); + TaskInput input(env_, &program_instruction, manip_, &seed, false, nullptr); FixStateCollisionProfile profile; @@ -152,12 +152,12 @@ TEST_F(FixStateCollisionProcessGeneratorUnit, WaypointInCollisionTest) EXPECT_TRUE(contacts.empty()); } -TEST_F(FixStateCollisionProcessGeneratorUnit, MoveWaypointFromCollisionRandomSamplerTest) +TEST_F(FixStateCollisionTaskGeneratorUnit, MoveWaypointFromCollisionRandomSamplerTest) { CompositeInstruction program = freespaceExampleProgramABB(); const Instruction program_instruction{ program }; Instruction seed = generateSkeletonSeed(program); - ProcessInput input(env_, &program_instruction, manip_, &seed, false, nullptr); + TaskInput input(env_, &program_instruction, manip_, &seed, false, nullptr); FixStateCollisionProfile profile; @@ -184,12 +184,12 @@ TEST_F(FixStateCollisionProcessGeneratorUnit, MoveWaypointFromCollisionRandomSam EXPECT_FALSE(WaypointInCollision(wp, input, profile, contacts)); } -TEST_F(FixStateCollisionProcessGeneratorUnit, MoveWaypointFromCollisionTrajoptTest) +TEST_F(FixStateCollisionTaskGeneratorUnit, MoveWaypointFromCollisionTrajoptTest) { CompositeInstruction program = freespaceExampleProgramABB(); const Instruction program_instruction{ program }; Instruction seed = generateSkeletonSeed(program); - ProcessInput input(env_, &program_instruction, manip_, &seed, false, nullptr); + TaskInput input(env_, &program_instruction, manip_, &seed, false, nullptr); FixStateCollisionProfile profile; diff --git a/tesseract/tesseract_planning/tesseract_process_managers/test/tesseract_process_managers_unit.cpp b/tesseract/tesseract_planning/tesseract_process_managers/test/tesseract_process_managers_unit.cpp index 96aa316ca28..7c66226ba97 100644 --- a/tesseract/tesseract_planning/tesseract_process_managers/test/tesseract_process_managers_unit.cpp +++ b/tesseract/tesseract_planning/tesseract_process_managers/test/tesseract_process_managers_unit.cpp @@ -12,7 +12,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #include #include #include @@ -25,7 +25,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include "raster_example_program.h" #include "raster_dt_example_program.h" @@ -87,7 +87,7 @@ class TesseractProcessManagerUnit : public ::testing::Test } }; -TEST_F(TesseractProcessManagerUnit, SeedMinLengthProcessGeneratorTest) +TEST_F(TesseractProcessManagerUnit, SeedMinLengthTaskGeneratorTest) { tesseract_planning::CompositeInstruction program = freespaceExampleProgramABB(); EXPECT_FALSE(program.getManipulatorInfo().empty()); @@ -104,22 +104,22 @@ TEST_F(TesseractProcessManagerUnit, SeedMinLengthProcessGeneratorTest) Instruction seed_instruction = seed; long current_length = getMoveInstructionCount(seed); - ProcessInput input(env_, &program_instruction, program.getManipulatorInfo(), &seed_instruction, true, nullptr); + TaskInput input(env_, &program_instruction, program.getManipulatorInfo(), &seed_instruction, true, nullptr); - SeedMinLengthProcessGenerator smlpg(current_length); + SeedMinLengthTaskGenerator smlpg(current_length); EXPECT_TRUE(smlpg.conditionalProcess(input, 1) == 1); long final_length = getMoveInstructionCount(*(input.getResults()->cast_const())); EXPECT_TRUE(final_length == current_length); - SeedMinLengthProcessGenerator smlpg2(2 * current_length); + SeedMinLengthTaskGenerator smlpg2(2 * current_length); EXPECT_TRUE(smlpg2.conditionalProcess(input, 2) == 1); long final_length2 = getMoveInstructionCount(*(input.getResults()->cast_const())); EXPECT_TRUE(final_length2 >= (2 * current_length)); seed_instruction = seed; - ProcessInput input2(env_, &program_instruction, program.getManipulatorInfo(), &seed_instruction, true, nullptr); + TaskInput input2(env_, &program_instruction, program.getManipulatorInfo(), &seed_instruction, true, nullptr); - SeedMinLengthProcessGenerator smlpg3(3 * current_length); + SeedMinLengthTaskGenerator smlpg3(3 * current_length); EXPECT_TRUE(smlpg3.conditionalProcess(input, 3) == 1); long final_length3 = getMoveInstructionCount(*(input2.getResults()->cast_const())); EXPECT_TRUE(final_length3 >= (3 * current_length)); diff --git a/tesseract_python/tesseract_python/swig/tesseract_process_managers_python.i b/tesseract_python/tesseract_python/swig/tesseract_process_managers_python.i index afc727d460b..dac311b1a41 100644 --- a/tesseract_python/tesseract_python/swig/tesseract_process_managers_python.i +++ b/tesseract_python/tesseract_python/swig/tesseract_process_managers_python.i @@ -86,14 +86,14 @@ // tesseract_process_managers -#include -#include +#include +#include #include #include #include -#include -#include +#include +#include #include #include @@ -131,13 +131,13 @@ %include "tesseract_motion_planners/core/profile_dictionary.h" -%include "tesseract_process_managers/core/process_interface.h" -%include "tesseract_process_managers/core/process_info.h" +%include "tesseract_process_managers/core/task_info.h" +%include "tesseract_process_managers/core/taskflow_interface.h" %include "tesseract_process_managers/core/process_planning_request.h" %include "tesseract_process_managers/core/process_planning_future.h" %include "tesseract_process_managers/core/process_planning_server.h" -//%include "tesseract_process_managers/process_generators/profile_switch_process_generator.h" -//%include "tesseract_process_managers/process_generators/iterative_spline_parameterization_process_generator.h" +//%include "tesseract_process_managers/task_generators/profile_switch_task_generator.h" +//%include "tesseract_process_managers/task_generators/iterative_spline_parameterization_task_generator.h"