From e5f06ebfdfa0eb6000ffc6d88f6147ecd2cf51a4 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 6 Nov 2024 22:24:54 -0600 Subject: [PATCH] Add FormatPlanningInputTask --- tesseract_motion_planners/core/src/utils.cpp | 3 +- .../config/task_composer_plugins.yaml | 198 ++++++++++++++++++ .../planning/CMakeLists.txt | 1 + .../nodes/format_planning_input_task.h | 86 ++++++++ ...ng_task_composer_core_plugin_factories.cpp | 4 + .../src/nodes/format_planning_input_task.cpp | 147 +++++++++++++ .../tesseract_task_composer_planning_unit.cpp | 126 ++++++++++- 7 files changed, 562 insertions(+), 3 deletions(-) create mode 100644 tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_planning_input_task.h create mode 100644 tesseract_task_composer/planning/src/nodes/format_planning_input_task.cpp diff --git a/tesseract_motion_planners/core/src/utils.cpp b/tesseract_motion_planners/core/src/utils.cpp index 309e346a8d..b549846346 100644 --- a/tesseract_motion_planners/core/src/utils.cpp +++ b/tesseract_motion_planners/core/src/utils.cpp @@ -267,7 +267,8 @@ bool formatProgramHelper(CompositeInstruction& composite_instructions, joint_names = it->second; } - if (base_instruction.getWaypoint().isStateWaypoint() || base_instruction.getWaypoint().isJointWaypoint()) + auto& wp = base_instruction.getWaypoint(); + if (wp.isStateWaypoint() || wp.isJointWaypoint() || wp.isCartesianWaypoint()) { if (formatJointPosition(joint_names, base_instruction.getWaypoint())) format_required = true; diff --git a/tesseract_task_composer/config/task_composer_plugins.yaml b/tesseract_task_composer/config/task_composer_plugins.yaml index 1a93d8cc1b..59869e1a16 100644 --- a/tesseract_task_composer/config/task_composer_plugins.yaml +++ b/tesseract_task_composer/config/task_composer_plugins.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] TrajOptIfoptTask: @@ -688,6 +754,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: TrajOptIfoptTask config: @@ -695,6 +770,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] CartesianTask: @@ -805,6 +882,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: @@ -812,6 +898,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] FreespaceTask: @@ -922,6 +1010,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: @@ -929,6 +1026,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] FreespaceIfoptTask: @@ -1039,6 +1138,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: FreespaceIfoptTask config: @@ -1046,6 +1154,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtTask: @@ -1133,6 +1243,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: @@ -1140,6 +1259,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtTask: @@ -1227,6 +1348,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: @@ -1234,6 +1364,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtOnlyTask: @@ -1314,6 +1446,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: @@ -1321,6 +1462,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtOnlyTask: @@ -1401,6 +1544,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: @@ -1408,6 +1560,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtGlobalTask: @@ -1511,6 +1665,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: @@ -1518,6 +1681,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtGlobalTask: @@ -1621,6 +1786,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: @@ -1628,6 +1802,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtOnlyGlobalTask: @@ -1724,6 +1900,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: @@ -1731,6 +1916,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtOnlyGlobalTask: @@ -1827,6 +2014,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: @@ -1834,5 +2030,7 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] diff --git a/tesseract_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index 7b58759571..3fc1ce07ad 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -20,6 +20,7 @@ set(LIB_SOURCE_FILES src/nodes/fix_state_collision_task.cpp src/nodes/format_as_input_task.cpp src/nodes/format_as_result_task.cpp + src/nodes/format_planning_input_task.cpp src/nodes/profile_switch_task.cpp src/nodes/min_length_task.cpp src/nodes/process_planning_input_task.cpp diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_planning_input_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_planning_input_task.h new file mode 100644 index 0000000000..0b888cc92b --- /dev/null +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/format_planning_input_task.h @@ -0,0 +1,86 @@ +/** + * @file format_planning_input_task.h + * + * @brief This is used to format a composite instruction for motion planning. C + * Currently it reformats joint waypoint, state waypoint and cartesian seed such + * that it aligns with the manipulator joint names ordering. + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_TASK_COMPOSERFORMAT_PLANNING_INPUT_TASK_H +#define TESSERACT_TASK_COMPOSERFORMAT_PLANNING_INPUT_TASK_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TaskComposerPluginFactory; + +class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT FormatPlanningInputTask : public TaskComposerTask +{ +public: + // Requried + static const std::string INOUT_PROGRAM_PORT; + static const std::string INPUT_ENVIRONMENT_PORT; + + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + FormatPlanningInputTask(); + explicit FormatPlanningInputTask(std::string name, + std::string input_program_key, + std::string input_environment_key, + std::string output_program_key, + bool conditional = false); + explicit FormatPlanningInputTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& plugin_factory); + ~FormatPlanningInputTask() override = default; + FormatPlanningInputTask(const FormatPlanningInputTask&) = delete; + FormatPlanningInputTask& operator=(const FormatPlanningInputTask&) = delete; + FormatPlanningInputTask(FormatPlanningInputTask&&) = delete; + FormatPlanningInputTask& operator=(FormatPlanningInputTask&&) = delete; + + bool operator==(const FormatPlanningInputTask& rhs) const; + bool operator!=(const FormatPlanningInputTask& rhs) const; + +protected: + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT + + static TaskComposerNodePorts ports(); + + std::unique_ptr + runImpl(TaskComposerContext& context, OptionalTaskComposerExecutor executor = std::nullopt) const override final; +}; + +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::FormatPlanningInputTask) + +#endif // TESSERACT_TASK_COMPOSERFORMAT_PLANNING_INPUT_TASK_H diff --git a/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp b/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp index 07c3574fbb..0199b23727 100644 --- a/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp +++ b/tesseract_task_composer/planning/src/factories/planning_task_composer_core_plugin_factories.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ using FixStateBoundsTaskFactory = TaskComposerTaskFactory; using FixStateCollisionTaskFactory = TaskComposerTaskFactory; using FormatAsInputTaskFactory = TaskComposerTaskFactory; using FormatAsResultTaskFactory = TaskComposerTaskFactory; +using FormatPlanningInputTaskFactory = TaskComposerTaskFactory; using MinLengthTaskFactory = TaskComposerTaskFactory; using ProfileSwitchTaskFactory = TaskComposerTaskFactory; using UpsampleTrajectoryTaskFactory = TaskComposerTaskFactory; @@ -79,6 +81,8 @@ TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FormatAsInputTaskFac // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FormatAsResultTaskFactory, FormatAsResultTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::FormatPlanningInputTaskFactory, FormatPlanningInputTaskFactory) +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::MinLengthTaskFactory, MinLengthTaskFactory) // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_TASK_COMPOSER_NODE_PLUGIN(tesseract_planning::ProfileSwitchTaskFactory, ProfileSwitchTaskFactory) diff --git a/tesseract_task_composer/planning/src/nodes/format_planning_input_task.cpp b/tesseract_task_composer/planning/src/nodes/format_planning_input_task.cpp new file mode 100644 index 0000000000..a5905c1ee1 --- /dev/null +++ b/tesseract_task_composer/planning/src/nodes/format_planning_input_task.cpp @@ -0,0 +1,147 @@ +/** + * @file format_planning_input_task.h + * + * @brief This is used to format a composite instruction for motion planning. C + * Currently it reformats joint waypoint, state waypoint and cartesian seed such + * that it aligns with the manipulator joint names ordering. + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +// Requried +const std::string FormatPlanningInputTask::INOUT_PROGRAM_PORT = "program"; +const std::string FormatPlanningInputTask::INPUT_ENVIRONMENT_PORT = "environment"; + +FormatPlanningInputTask::FormatPlanningInputTask() + : TaskComposerTask("FormatPlanningInputTask", FormatPlanningInputTask::ports(), false) +{ +} + +FormatPlanningInputTask::FormatPlanningInputTask(std::string name, + std::string input_program_key, + std::string input_environment_key, + std::string output_program_key, + bool is_conditional) + : TaskComposerTask(std::move(name), FormatPlanningInputTask::ports(), is_conditional) +{ + input_keys_.add(INOUT_PROGRAM_PORT, std::move(input_program_key)); + input_keys_.add(INPUT_ENVIRONMENT_PORT, std::move(input_environment_key)); + output_keys_.add(INOUT_PROGRAM_PORT, std::move(output_program_key)); + + validatePorts(); +} + +FormatPlanningInputTask::FormatPlanningInputTask(std::string name, + const YAML::Node& config, + const TaskComposerPluginFactory& /*plugin_factory*/) + : TaskComposerTask(std::move(name), FormatPlanningInputTask::ports(), config) +{ +} + +TaskComposerNodePorts FormatPlanningInputTask::ports() +{ + TaskComposerNodePorts ports; + ports.input_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; + ports.input_required[INPUT_ENVIRONMENT_PORT] = TaskComposerNodePorts::SINGLE; + ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; + return ports; +} + +std::unique_ptr FormatPlanningInputTask::runImpl(TaskComposerContext& context, + OptionalTaskComposerExecutor /*executor*/) const +{ + // -------------------- + // Check that inputs are valid + // -------------------- + auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT); + if (env_poly.getType() != std::type_index(typeid(std::shared_ptr))) + { + auto info = std::make_unique(*this); + info->return_value = 0; + info->status_code = 0; + info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type"; + CONSOLE_BRIDGE_logError("%s", info->status_message.c_str()); + return info; + } + + auto env = env_poly.as>(); + + auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT); + if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction))) + { + auto info = std::make_unique(*this); + info->return_value = 0; + info->status_code = 0; + info->status_message = "Input to FormatPlanningInputTask must be a composite instruction"; + CONSOLE_BRIDGE_logError("%s", info->status_message.c_str()); + return info; + } + + auto& ci = input_data_poly.as(); + + const bool formatting_required = formatProgram(ci, *env); + setData(*context.data_storage, INOUT_PROGRAM_PORT, ci); + + auto info = std::make_unique(*this); + info->return_value = 1; + info->status_code = 1; + if (formatting_required) + { + info->color = "yellow"; + info->status_message = "Successful (Formatting Required)"; + } + else + { + info->color = "green"; + info->status_message = "Successful"; + } + return info; +} + +bool FormatPlanningInputTask::operator==(const FormatPlanningInputTask& rhs) const +{ + return (TaskComposerNode::operator==(rhs)); +} +bool FormatPlanningInputTask::operator!=(const FormatPlanningInputTask& rhs) const { return !operator==(rhs); } + +template +void FormatPlanningInputTask::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskComposerTask); +} + +} // namespace tesseract_planning + +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FormatPlanningInputTask) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FormatPlanningInputTask) diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index 58aa4e7e10..bd046c0830 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -10,6 +10,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -974,6 +975,127 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMinLengthTaskTests) // NO } } +TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFormatPlanningInputTaskTests) // NOLINT +{ + { // Construction + FormatPlanningInputTask task; + EXPECT_EQ(task.getName(), "FormatPlanningInputTask"); + EXPECT_EQ(task.isConditional(), false); + } + + { // Construction + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: + program: input_data + environment: environment + outputs: + program: output_data)"; + YAML::Node config = YAML::Load(str); + FormatPlanningInputTask task("abc", config["config"], factory); + EXPECT_EQ(task.getName(), "abc"); + EXPECT_EQ(task.isConditional(), true); + EXPECT_EQ(task.getInputKeys().size(), 2); + EXPECT_EQ(task.getInputKeys().get(FormatPlanningInputTask::INOUT_PROGRAM_PORT), "input_data"); + EXPECT_EQ(task.getInputKeys().get(FormatPlanningInputTask::INPUT_ENVIRONMENT_PORT), "environment"); + EXPECT_EQ(task.getOutputKeys().size(), 1); + EXPECT_EQ(task.getOutputKeys().get(MinLengthTask::INOUT_PROGRAM_PORT), "output_data"); + EXPECT_EQ(task.getOutboundEdges().size(), 0); + EXPECT_EQ(task.getInboundEdges().size(), 0); + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true)"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + inputs: + program: input_data + environment: environment)"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Construction failure + TaskComposerPluginFactory factory; + std::string str = R"(config: + conditional: true + outputs: + program: output_data)"; + YAML::Node config = YAML::Load(str); + EXPECT_ANY_THROW(std::make_unique("abc", config["config"], factory)); // NOLINT + } + + { // Serialization + auto task = std::make_unique(); + + // Serialization + test_suite::runSerializationPointerTest(task, "TaskComposerFormatPlanningInputTaskTests"); + } + + { // Test run method + auto data = std::make_unique(); + auto input_data = test_suite::jointInterpolateExampleProgramABB(true); + data->setData("input_data", input_data); + data->setData("environment", std::shared_ptr(env_)); + auto context = std::make_unique("abc", std::move(data)); + FormatPlanningInputTask task("abc", "input_data", "environment", "output_data", true); + EXPECT_EQ(task.run(*context), 1); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "green"); + EXPECT_EQ(node_info->return_value, 1); + EXPECT_EQ(node_info->status_code, 1); + EXPECT_EQ(node_info->status_message.empty(), false); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_GE(context->data_storage->getData("output_data").as().size(), input_data.size()); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } + + { // Failure missing input data + auto data = std::make_unique(); + data->setData("environment", std::shared_ptr(env_)); + auto context = std::make_unique("abc", std::move(data)); + FormatPlanningInputTask task("abc", "input_data", "environment", "output_data", true); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "red"); + EXPECT_EQ(node_info->return_value, 0); + EXPECT_EQ(node_info->status_code, -1); + EXPECT_EQ(node_info->status_message.empty(), false); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } + + { // Failure missing environment data + auto data = std::make_unique(); + data->setData("input_data", test_suite::jointInterpolateExampleProgramABB(true)); + auto context = std::make_unique("abc", std::move(data)); + FormatPlanningInputTask task("abc", "input_data", "environment", "output_data", true); + EXPECT_EQ(task.run(*context), 0); + auto node_info = context->task_infos.getInfo(task.getUUID()); + EXPECT_EQ(node_info->color, "red"); + EXPECT_EQ(node_info->return_value, 0); + EXPECT_EQ(node_info->status_code, -1); + EXPECT_EQ(node_info->status_message.empty(), false); + EXPECT_EQ(node_info->isAborted(), false); + EXPECT_EQ(context->isAborted(), false); + EXPECT_EQ(context->isSuccessful(), true); + EXPECT_TRUE(context->task_infos.getAbortingNode().is_nil()); + } +} + TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) // NOLINT { { // Construction @@ -2984,7 +3106,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterMotionTaskTests) // EXPECT_TRUE(future->context->data_storage->hasKey(output_key)); EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); auto info_map = future->context->task_infos.getInfoMap(); - EXPECT_EQ(info_map.size(), 76); + EXPECT_EQ(info_map.size(), 77); // Make sure keys are unique std::vector raster_input_keys; @@ -3451,7 +3573,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRasterOnlyMotionTaskTests) EXPECT_TRUE(future->context->data_storage->hasKey(output_key)); EXPECT_TRUE(future->context->task_infos.getAbortingNode().is_nil()); auto info_map = future->context->task_infos.getInfoMap(); - EXPECT_EQ(info_map.size(), 60); + EXPECT_EQ(info_map.size(), 61); // Make sure keys are unique std::vector raster_input_keys;