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 84fe4b5b0f6..10195da8052 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 @@ -73,7 +73,8 @@ class TaskComposerNode TaskComposerNode(std::string name = "TaskComposerNode", TaskComposerNodeType type = TaskComposerNodeType::NODE, TaskComposerNodePorts ports = TaskComposerNodePorts(), - bool conditional = false); + bool conditional = false, + int return_value_override = -1); explicit TaskComposerNode(std::string name, TaskComposerNodeType type, TaskComposerNodePorts ports, @@ -220,6 +221,13 @@ class TaskComposerNode /** @brief The nodes ports definition */ TaskComposerNodePorts ports_; + /** + * @brief Value with which to override the return value nominally provided by runImpl() in the TaskComposerNodeInfo. + * @details The return value provided by runImpl() will be overriden with this value when it is >= 0. + * This value can be set to force a task to behave in a specific way (e.g., for debugging or unit tests). + */ + int return_value_override_{ -1 }; + /** @brief Indicate if task triggers abort */ bool trigger_abort_{ false }; diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 86ba0f53529..bda5f72eb24 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -87,7 +87,8 @@ namespace tesseract_planning TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, TaskComposerNodePorts ports, - bool conditional) + bool conditional, + int return_value_override) : name_(std::move(name)) , ns_(name_) , type_(type) @@ -95,6 +96,7 @@ TaskComposerNode::TaskComposerNode(std::string name, , uuid_str_(boost::uuids::to_string(uuid_)) , conditional_(conditional) , ports_(std::move(ports)) + , return_value_override_(return_value_override) { } @@ -126,6 +128,9 @@ TaskComposerNode::TaskComposerNode(std::string name, output_keys_ = n.as(); } + + if (YAML::Node n = config["return_value_override"]) + return_value_override_ = n.as(); } catch (const std::exception& e) { @@ -158,6 +163,12 @@ int TaskComposerNode::run(TaskComposerContext& context, OptionalTaskComposerExec try { results = runImpl(context, executor); + if (return_value_override_ >= 0) + { + results.return_value = return_value_override_; + results.status_message += " (Return value overridden to " + std::to_string(return_value_override_) + ")"; + results.color = "yellow"; + } } catch (const std::exception& e) {