diff --git a/.github/workflows/mac.yml b/.github/workflows/mac.yml index c37354106e..5290260b64 100644 --- a/.github/workflows/mac.yml +++ b/.github/workflows/mac.yml @@ -35,7 +35,7 @@ jobs: fail-fast: false matrix: config: - - runner: macos-12 + - runner: macos-13 vcpkg_triplet: x64-osx-dynamic-release arch: x64 homebrew_root: /usr/local diff --git a/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml b/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml index a2f00db1ee..69282ef8ac 100644 --- a/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml +++ b/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml @@ -106,6 +106,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesFTask config: @@ -113,6 +122,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] DescartesDTask: @@ -207,6 +218,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesDTask config: @@ -214,6 +234,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] DescartesFNPCTask: @@ -296,6 +318,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesFNPCTask config: @@ -303,6 +334,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] DescartesDNPCTask: @@ -385,6 +418,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesDNPCTask config: @@ -392,6 +434,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] OMPLTask: @@ -486,6 +530,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: OMPLTask config: @@ -493,6 +546,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] TrajOptTask: @@ -587,6 +642,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: TrajOptTask config: @@ -594,6 +658,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] CartesianTask: @@ -704,6 +770,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: CartesianTask config: @@ -711,6 +786,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] FreespaceTask: @@ -821,6 +898,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: FreespaceTask config: @@ -828,6 +914,136 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] + FreespaceIfoptTask: + class: PipelineTaskFactory + config: + conditional: true + inputs: + program: input_data + outputs: + program: output_data + nodes: + DoneTask: + class: DoneTaskFactory + config: + conditional: false + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + MinLengthTask: + class: MinLengthTaskFactory + config: + conditional: true + inputs: + program: input_data + environment: environment + profiles: profiles + composite_profile_remapping: composite_profile_remapping + outputs: + program: output_data + OMPLMotionPlannerTask: + class: OMPLMotionPlannerTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + composite_profile_remapping: composite_profile_remapping + move_profile_remapping: move_profile_remapping + outputs: + program: output_data + format_result_as_input: true + TrajOptIfoptMotionPlannerTask: + class: TrajOptIfoptMotionPlannerTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + composite_profile_remapping: composite_profile_remapping + move_profile_remapping: move_profile_remapping + outputs: + program: output_data + format_result_as_input: false + DiscreteContactCheckTask: + class: DiscreteContactCheckTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + composite_profile_remapping: composite_profile_remapping + IterativeSplineParameterizationTask: + class: IterativeSplineParameterizationTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + composite_profile_remapping: composite_profile_remapping + move_profile_remapping: move_profile_remapping + outputs: + program: output_data + edges: + - source: MinLengthTask + destinations: [ErrorTask, OMPLMotionPlannerTask] + - source: OMPLMotionPlannerTask + destinations: [ErrorTask, TrajOptIfoptMotionPlannerTask] + - source: TrajOptIfoptMotionPlannerTask + destinations: [ErrorTask, DiscreteContactCheckTask] + - source: DiscreteContactCheckTask + destinations: [ErrorTask, IterativeSplineParameterizationTask] + - source: IterativeSplineParameterizationTask + destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] + FreespaceIfoptPipeline: + class: PipelineTaskFactory + config: + conditional: false + inputs: + planning_input: planning_input + outputs: + program: output_data + nodes: + ProcessInputTask: + class: ProcessPlanningInputTaskFactory + config: + conditional: false + inputs: + planning_input: planning_input + outputs: + program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data + MotionPlanningTask: + task: FreespaceIfoptTask + config: + conditional: false + abort_terminal: 0 + edges: + - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtTask: @@ -915,6 +1131,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtTask config: @@ -922,6 +1147,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtTask: @@ -1009,6 +1236,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtTask config: @@ -1016,6 +1252,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtOnlyTask: @@ -1096,6 +1334,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtOnlyTask config: @@ -1103,6 +1350,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtOnlyTask: @@ -1183,6 +1432,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtOnlyTask config: @@ -1190,6 +1448,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtGlobalTask: @@ -1293,6 +1553,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtGlobalTask config: @@ -1300,6 +1569,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtGlobalTask: @@ -1403,6 +1674,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtGlobalTask config: @@ -1410,6 +1690,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtOnlyGlobalTask: @@ -1506,6 +1788,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtOnlyGlobalTask config: @@ -1513,6 +1804,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtOnlyGlobalTask: @@ -1609,6 +1902,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtOnlyGlobalTask config: @@ -1616,5 +1918,7 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h index 78d18cab27..e3ebefff37 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h @@ -148,12 +148,11 @@ class TaskComposerNode TaskComposerNodePorts getPorts() const; /** @brief Generate the Dotgraph as a string */ - std::string - getDotgraph(const std::map>& results_map = {}) const; + std::string getDotgraph(const std::map>& results_map) const; /** @brief Generate the Dotgraph and save to file */ bool saveDotgraph(const std::string& filepath, - const std::map>& results_map = {}) const; + const std::map>& results_map) const; /** @brief Rename input keys */ virtual void renameInputKeys(const std::map& input_keys); @@ -170,8 +169,14 @@ class TaskComposerNode */ virtual std::string dump(std::ostream& os, - const TaskComposerNode* parent = nullptr, - const std::map>& results_map = {}) const; + const TaskComposerNode* parent, + const std::map>& results_map) const; + + /** + * @brief dump the task to dot + * @brief Return additional subgraphs which should get appended if needed + */ + virtual std::string dump(std::ostream& os) const; bool operator==(const TaskComposerNode& rhs) const; bool operator!=(const TaskComposerNode& rhs) const; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h index d0287b4ef2..7e54b2e0f4 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_pipeline.h @@ -33,12 +33,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { class TaskComposerContext; class TaskComposerNode; -class TaskComposerNodeInfo; class TaskComposerExecutor; class TaskComposerPluginFactory; /** diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h index fe4a8a8131..a526bdabe9 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h @@ -100,8 +100,8 @@ class TaskComposerPluginFactory ~TaskComposerPluginFactory(); TaskComposerPluginFactory(const TaskComposerPluginFactory&) = delete; TaskComposerPluginFactory& operator=(const TaskComposerPluginFactory&) = delete; - TaskComposerPluginFactory(TaskComposerPluginFactory&&) = default; - TaskComposerPluginFactory& operator=(TaskComposerPluginFactory&&) = default; + TaskComposerPluginFactory(TaskComposerPluginFactory&&) noexcept; + TaskComposerPluginFactory& operator=(TaskComposerPluginFactory&&) noexcept; /** * @brief Load plugins from a configuration object diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 649c359dc9..87b37748e3 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -31,6 +31,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include #include #include #include @@ -435,6 +437,8 @@ void TaskComposerNode::renameOutputKeys(const std::map void TaskComposerNode::setConditional(bool enable) { conditional_ = enable; } +std::string TaskComposerNode::dump(std::ostream& os) const { return dump(os, nullptr, {}); } + std::string TaskComposerNode::dump(std::ostream& os, const TaskComposerNode* /*parent*/, @@ -543,18 +547,9 @@ void TaskComposerNode::serialize(Archive& ar, const unsigned int /*version*/) std::string TaskComposerNode::toString(const boost::uuids::uuid& u, const std::string& prefix) { - std::string result; - result.reserve(36); - - std::size_t i = 0; - for (const auto* it_data = u.begin(); it_data != u.end(); ++it_data, ++i) - { - const size_t hi = ((*it_data) >> 4) & 0x0F; - result += boost::uuids::detail::to_char(hi); + auto result = boost::lexical_cast(u); + boost::replace_all(result, "-", ""); - const size_t lo = (*it_data) & 0x0F; - result += boost::uuids::detail::to_char(lo); - } return (prefix + result); } diff --git a/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp b/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp index 1e261ce958..f52e066892 100644 --- a/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp +++ b/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp @@ -91,6 +91,8 @@ TaskComposerPluginFactory::TaskComposerPluginFactory(const std::string& config) // This prevents it from being defined inline. // If not the forward declare of PluginLoader cause compiler error. TaskComposerPluginFactory::~TaskComposerPluginFactory() = default; +TaskComposerPluginFactory::TaskComposerPluginFactory(TaskComposerPluginFactory&&) noexcept = default; +TaskComposerPluginFactory& TaskComposerPluginFactory::operator=(TaskComposerPluginFactory&&) noexcept = default; void TaskComposerPluginFactory::loadConfig(const tesseract_common::TaskComposerPluginInfo& config) {