From 69de947b1ac0df4bfc45c854be8a9f50689c60a7 Mon Sep 17 00:00:00 2001 From: Tyler Marr <41449746+marrts@users.noreply.github.com> Date: Thu, 11 Jan 2024 11:05:37 -0600 Subject: [PATCH] Add optional namespace field to task nodes (#433) --- tesseract_task_composer/README.rst | 123 +++++++++++++++++- .../core/task_composer_node.h | 9 ++ .../core/src/task_composer_node.cpp | 9 +- .../planning/nodes/motion_planner_task.hpp | 4 +- .../planning/src/nodes/check_input_task.cpp | 6 +- .../nodes/continuous_contact_check_task.cpp | 6 +- .../src/nodes/discrete_contact_check_task.cpp | 6 +- .../src/nodes/fix_state_bounds_task.cpp | 6 +- .../src/nodes/fix_state_collision_task.cpp | 6 +- ...iterative_spline_parameterization_task.cpp | 12 +- .../planning/src/nodes/min_length_task.cpp | 8 +- .../src/nodes/profile_switch_task.cpp | 6 +- .../ruckig_trajectory_smoothing_task.cpp | 12 +- .../time_optimal_parameterization_task.cpp | 6 +- .../src/nodes/upsample_trajectory_task.cpp | 6 +- 15 files changed, 179 insertions(+), 46 deletions(-) diff --git a/tesseract_task_composer/README.rst b/tesseract_task_composer/README.rst index 216d197e2d..8528b65cde 100644 --- a/tesseract_task_composer/README.rst +++ b/tesseract_task_composer/README.rst @@ -53,12 +53,14 @@ This file allows you define Excutors and Tasks (aka Nodes). MinLengthTask: class: MinLengthTaskFactory config: + namespace: MinLengthTask # (optional, defaults to parent yaml key name "MinLengthTask") conditional: true inputs: [input_data] outputs: [output_data] DescartesMotionPlannerTask: class: DescartesFMotionPlannerTaskFactory config: + namespace: DescartesMotionPlannerTask # (optional, defaults to parent yaml key name "DescartesMotionPlannerTask") conditional: true inputs: [output_data] outputs: [output_data] @@ -66,11 +68,13 @@ This file allows you define Excutors and Tasks (aka Nodes). DiscreteContactCheckTask: class: DiscreteContactCheckTaskFactory config: + namespace: DiscreteContactCheckTask # (optional, defaults to parent yaml key name "DiscreteContactCheckTask") conditional: true inputs: [output_data] IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: + namespace: IterativeSplineParameterizationTask # (optional, defaults to parent yaml key name "IterativeSplineParameterizationTask") conditional: true inputs: [output_data] outputs: [output_data] @@ -150,12 +154,14 @@ Define the graph nodes and edges as shown in the config below. MinLengthTask: class: MinLengthTaskFactory config: + namespace: MinLengthTask # (optional) conditional: true inputs: [input_data] outputs: [output_data] DescartesMotionPlannerTask: class: DescartesFMotionPlannerTaskFactory config: + namespace: DescartesMotionPlannerTask # (optional) conditional: true inputs: [output_data] outputs: [output_data] @@ -163,6 +169,7 @@ Define the graph nodes and edges as shown in the config below. TrajOptMotionPlannerTask: class: TrajOptMotionPlannerTaskFactory config: + namespace: TrajOptMotionPlannerTask # (optional) conditional: true inputs: [output_data] outputs: [output_data] @@ -170,11 +177,13 @@ Define the graph nodes and edges as shown in the config below. DiscreteContactCheckTask: class: DiscreteContactCheckTaskFactory config: + namespace: DiscreteContactCheckTask # (optional) conditional: true inputs: [output_data] IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: + namespace: IterativeSplineParameterizationTask # (optional) conditional: true inputs: [output_data] outputs: [output_data] @@ -191,7 +200,7 @@ Define the graph nodes and edges as shown in the config below. destinations: [ErrorTask, DoneTask] terminals: [ErrorTask, DoneTask] -Leveraging a perviously defined task +Leveraging a previously defined task ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ When using a perviously defined task it is referenced using `task:` instead of `class:`. @@ -226,6 +235,97 @@ Also you can indicate that it should abort if a terminal is reached by specifyin destinations: [CartesianPipelineTask] terminals: [CartesianPipelineTask] + +Reusing a namespace across multiple tasks +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Sometimes it is desireable to reuse a particular configration of a task. To prevent the need from having to make two entries for the tasks you can use the namespace field under the task config. + +Here is an example where the namespace field is used to reuse a contact check configuration. + +.. code-block:: yaml + + task_composer_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_task_composer_factories + executors: + default: TaskflowExecutor + plugins: + TaskflowExecutor: + class: TaskflowTaskComposerExecutorFactory + config: + threads: 5 + tasks: + plugins: + FreespacePipeline: + class: GraphTaskFactory + config: + inputs: [input_data] + outputs: [output_data] + nodes: + DoneTask: + class: DoneTaskFactory + config: + conditional: false + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + MinLengthTask: + class: MinLengthTaskFactory + config: + conditional: true + inputs: [input_data] + outputs: [output_data] + TrajOptMotionPlannerTask: + class: TrajOptMotionPlannerTaskFactory + config: + conditional: true + inputs: [output_data] + outputs: [output_data] + format_result_as_input: false + ContactCheckTask_1: + class: DiscreteContactCheckTaskFactory + config: + namespace: DiscreteContactCheckTask + conditional: true + inputs: [output_data] + OMPLMotionPlannerTask: + class: OMPLMotionPlannerTaskFactory + config: + conditional: true + inputs: [input_data] + outputs: [output_data] + format_result_as_input: false + ContactCheckTask_2: + class: DiscreteContactCheckTaskFactory + config: + namespace: DiscreteContactCheckTask + conditional: true + inputs: [output_data] + IterativeSplineParameterizationTask: + class: IterativeSplineParameterizationTaskFactory + config: + conditional: true + inputs: [output_data] + outputs: [output_data] + edges: + - source: MinLengthTask + destinations: [ErrorTask, TrajOptMotionPlannerTask] + - source: TrajOptMotionPlannerTask + destinations: [OMPLMotionPlannerTask, ContactCheckTask_1] + - source: ContactCheckTask_1 + destinations: [OMPLMotionPlannerTask, IterativeSplineParameterizationTask] + - source: OMPLMotionPlannerTask + destinations: [ErrorTask, ContactCheckTask_2] + - source: ContactCheckTask_2 + destinations: [ErrorTask, IterativeSplineParameterizationTask] + - source: IterativeSplineParameterizationTask + destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] + Descartes Motion Planner Task ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -238,6 +338,7 @@ Task for running Descartes motion planner DescartesMotionPlannerTask: class: DescartesDMotionPlannerTaskFactory config: + namespace: DescartesMotionPlannerTask # (optional, defaults to parent yaml key name "DescartesMotionPlannerTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -251,6 +352,7 @@ Task for running Descartes motion planner DescartesMotionPlannerTask: class: DescartesFMotionPlannerTaskFactory config: + namespace: DescartesMotionPlannerTask # (optional, defaults to parent yaml key name "DescartesMotionPlannerTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -266,6 +368,7 @@ Task for running OMPL motion planner OMPLMotionPlannerTask: class: OMPLMotionPlannerTaskFactory config: + namespace: OMPLMotionPlannerTask # (optional, defaults to parent yaml key name "OMPLMotionPlannerTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -281,6 +384,7 @@ Task for running TrajOpt motion planner TrajOptMotionPlannerTask: class: TrajOptMotionPlannerTaskFactory config: + namespace: TrajOptMotionPlannerTask # (optional, defaults to parent yaml key name "TrajOptMotionPlannerTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -296,6 +400,7 @@ Task for running TrajOpt Ifopt motion planner TrajOptIfoptMotionPlannerTask: class: TrajOptIfoptMotionPlannerTaskFactory config: + namespace: TrajOptIfoptMotionPlannerTask # (optional, defaults to parent yaml key name "TrajOptIfoptMotionPlannerTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -311,6 +416,7 @@ Task for running Simple motion planner SimpleMotionPlannerTask: class: SimpleMotionPlannerTaskFactory config: + namespace: SimpleMotionPlannerTask # (optional, defaults to parent yaml key name "SimpleMotionPlannerTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -326,6 +432,7 @@ Perform iterative spline time parameterization IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: + namespace: IterativeSplineParameterizationTask # (optional, defaults to parent yaml key name "IterativeSplineParameterizationTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -341,6 +448,7 @@ Perform time optimal time parameterization TimeOptimalParameterizationTask: class: TimeOptimalParameterizationTaskFactory config: + namespace: TimeOptimalParameterizationTask # (optional, defaults to parent yaml key name "TimeOptimalParameterizationTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -355,6 +463,7 @@ Perform trajectory smoothing leveraging Ruckig. Time parameterization must be ra RuckigTrajectorySmoothingTask: class: RuckigTrajectorySmoothingTaskFactory config: + namespace: RuckigTrajectorySmoothingTask # (optional, defaults to parent yaml key name "RuckigTrajectorySmoothingTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -421,9 +530,10 @@ Task for checking input data structure .. code-block:: yaml - MinLengthTask: - class: MinLengthTaskFactory + CheckInputTask: + class: CheckInputTaskFactory config: + namespace: CheckInputTask # (optional, defaults to parent yaml key name "CheckInputTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -438,6 +548,7 @@ Continuous collision check trajectory task ContinuousContactCheckTask: class: ContinuousContactCheckTaskFactory config: + namespace: ContinuousContactCheckTask # (optional, defaults to parent yaml key name "ContinuousContactCheckTask") conditional: true inputs: [input_data] @@ -451,6 +562,7 @@ Discrete collision check trajectory task DiscreteContactCheckTask: class: DiscreteContactCheckTaskFactory config: + namespace: DiscreteContactCheckTask # (optional, defaults to parent yaml key name "DiscreteContactCheckTask") conditional: true inputs: [input_data] @@ -516,6 +628,7 @@ This task modifies the input instructions in order to push waypoints that are ou FixStateBoundsTask: class: FixStateBoundsTaskFactory config: + namespace: FixStateBoundsTask # (optional, defaults to parent yaml key name "FixStateBoundsTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -533,6 +646,7 @@ This task modifies the input instructions in order to push waypoints that are in FixStateCollisionTask: class: FixStateCollisionTaskFactory config: + namespace: FixStateCollisionTask # (optional, defaults to parent yaml key name "FixStateCollisionTask") conditional: true inputs: [input_data] outputs: [output_data] @@ -547,6 +661,7 @@ Task for processing the input data so it meets a minimum length. Planners like t MinLengthTask: class: MinLengthTaskFactory config: + namespace: MinLengthTask # (optional, defaults to parent yaml key name "MinLengthTask") conditional: false inputs: [input_data] outputs: [output_data] @@ -561,6 +676,7 @@ This task simply returns a value specified in the composite profile. This can be ProfileSwitchTask: class: ProfileSwitchTaskFactory config: + namespace: ProfileSwitchTask # (optional, defaults to parent yaml key name "ProfileSwitchTask") conditional: false inputs: [input_data] @@ -577,6 +693,7 @@ This is used to upsample the results trajectory based on the longest valid segme UpsampleTrajectoryTask: class: UpsampleTrajectoryTaskFactory config: + namespace: UpsampleTrajectoryTask # (optional, defaults to parent yaml key name "UpsampleTrajectoryTask") conditional: false inputs: [input_data] outputs: [output_data] 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 23b0905e31..989de783a4 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 @@ -71,6 +71,12 @@ class TaskComposerNode /** @brief The name of the node */ const std::string& getName() const; + /** @brief Set the namespace of the node */ + void setNamespace(const std::string& ns); + + /** @brief The namespace of the node */ + const std::string& getNamespace() const; + /** @brief The node type TASK, GRAPH, etc */ TaskComposerNodeType getType() const; @@ -142,6 +148,9 @@ class TaskComposerNode /** @brief The name of the task */ std::string name_; + /** @brief The namespace of the task */ + std::string ns_; + /** @brief The node type */ TaskComposerNodeType type_; diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 2128fc2f54..62b3ed3ba2 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -39,6 +39,7 @@ namespace tesseract_planning { TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, bool conditional) : name_(std::move(name)) + , ns_(name_) , type_(type) , uuid_(boost::uuids::random_generator()()) , uuid_str_(boost::uuids::to_string(uuid_)) @@ -51,6 +52,8 @@ TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, { try { + ns_ = config["namespace"].IsDefined() ? config["namespace"].as() : name_; + if (YAML::Node n = config["conditional"]) conditional_ = n.as(); @@ -79,6 +82,9 @@ TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, void TaskComposerNode::setName(const std::string& name) { name_ = name; } const std::string& TaskComposerNode::getName() const { return name_; } +void TaskComposerNode::setNamespace(const std::string& ns) { ns_ = ns; } +const std::string& TaskComposerNode::getNamespace() const { return ns_; } + TaskComposerNodeType TaskComposerNode::getType() const { return type_; } const boost::uuids::uuid& TaskComposerNode::getUUID() const { return uuid_; } @@ -134,7 +140,8 @@ std::string TaskComposerNode::dump(std::ostream& os, if (conditional_) { - os << std::endl << tmp << " [shape=diamond, label=\"" << name_ << "\\n(" << uuid_str_ << ")"; + os << std::endl << tmp << " [shape=diamond, label=\"" << name_ << "\\n"; + os << "Namespace: " << ns_ << "\\n(" << uuid_str_ << ")"; os << "\\n Inputs: ["; for (std::size_t i = 0; i < input_keys_.size(); ++i) diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp index eb71d0ed63..654f693eaa 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp @@ -52,7 +52,7 @@ class MotionPlannerTask : public TaskComposerTask bool format_result_as_input, bool conditional) : TaskComposerTask(std::move(name), conditional) - , planner_(std::make_shared(name_)) + , planner_(std::make_shared(ns_)) , format_result_as_input_(format_result_as_input) { input_keys_.push_back(std::move(input_key)); @@ -62,7 +62,7 @@ class MotionPlannerTask : public TaskComposerTask explicit MotionPlannerTask(std::string name, // NOLINT(performance-unnecessary-value-param) const YAML::Node& config, const TaskComposerPluginFactory& /*plugin_factory*/) - : TaskComposerTask(std::move(name), config), planner_(std::make_shared(name_)) + : TaskComposerTask(std::move(name), config), planner_(std::make_shared(ns_)) { if (input_keys_.empty()) throw std::runtime_error("MotionPlannerTask, config missing 'inputs' entry"); diff --git a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp index 2ab0c06fb6..1a0b12d7be 100644 --- a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp @@ -82,10 +82,10 @@ TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerContext& context, const auto& ci = input_data_poly.as(); std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); if (!cur_composite_profile->isValid(context)) { diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp index fe3d24d706..b6a91fdec7 100644 --- a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp @@ -91,11 +91,11 @@ TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerConte // Get Composite Profile const auto& ci = input_data_poly.as(); std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto default_profile = std::make_shared(); default_profile->config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - auto cur_composite_profile = getProfile(name_, profile, *problem.profiles, default_profile); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = getProfile(ns_, profile, *problem.profiles, default_profile); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(problem.manip_info); diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp index 48adbd072d..e3e0c2db76 100644 --- a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp @@ -90,10 +90,10 @@ TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerContext // Get Composite Profile const auto& ci = input_data_poly.as(); std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(problem.manip_info); diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp index 191bd38017..47215192fa 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp @@ -97,10 +97,10 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerContext& cont // Get Composite Profile std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); limits.joint_limits.col(0) = limits.joint_limits.col(0).array() + cur_composite_profile->lower_bounds_reduction; limits.joint_limits.col(1) = limits.joint_limits.col(1).array() - cur_composite_profile->upper_bounds_reduction; diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp index 468f6324bd..0e38c60c8f 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp @@ -367,10 +367,10 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerContext& c // Get Composite Profile std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); switch (cur_composite_profile->mode) { diff --git a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp index a89f4a2c43..568e260c17 100644 --- a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp @@ -115,10 +115,10 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp // Get Composite Profile std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); // Create data structures for checking for plan profile overrides auto flattened = ci.flatten(moveFilter); @@ -148,10 +148,10 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp std::string move_profile = mi.getProfile(); // Check for remapping of the plan profile - move_profile = getProfileString(name_, profile, problem.move_profile_remapping); + move_profile = getProfileString(ns_, profile, problem.move_profile_remapping); auto cur_move_profile = getProfile( - name_, move_profile, *problem.profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(name_, profile, cur_move_profile, mi.profile_overrides); + ns_, move_profile, *problem.profiles, std::make_shared()); + // cur_move_profile = applyProfileOverrides(ns_, profile, cur_move_profile, mi.profile_overrides); // If there is a move profile associated with it, override the parameters if (cur_move_profile) diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index 0cdf7aea2f..367d5779f4 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -91,10 +91,10 @@ TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerContext& context, const auto& ci = input_data_poly.as(); long cnt = ci.getMoveInstructionCount(); std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); if (cnt < cur_composite_profile->min_length) { @@ -108,7 +108,7 @@ TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerContext& context, request.env = problem.env; // Set up planner - SimpleMotionPlanner planner(name_); + SimpleMotionPlanner planner(ns_); auto profile = std::make_shared(subdivisions, subdivisions); diff --git a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp index 8c1e1108e4..719a3dc1a9 100644 --- a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp @@ -83,10 +83,10 @@ TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerContext& conte // Get Composite Profile const auto& ci = input_data_poly.as(); std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); // Return the value specified in the profile CONSOLE_BRIDGE_logDebug("ProfileSwitchProfile returning %d", cur_composite_profile->return_value); diff --git a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp index 7e60a00bb8..1c7b257f9d 100644 --- a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp @@ -100,10 +100,10 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerCo // Get Composite Profile std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); RuckigTrajectorySmoothing solver(cur_composite_profile->duration_extension_fraction, cur_composite_profile->max_duration_extension_factor); @@ -138,10 +138,10 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerCo std::string move_profile = mi.getProfile(); // Check for remapping of the plan profile - move_profile = getProfileString(name_, profile, problem.move_profile_remapping); + move_profile = getProfileString(ns_, profile, problem.move_profile_remapping); auto cur_move_profile = getProfile( - name_, move_profile, *problem.profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(name_, profile, cur_move_profile, mi.profile_overrides); + ns_, move_profile, *problem.profiles, std::make_shared()); + // cur_move_profile = applyProfileOverrides(ns_, profile, cur_move_profile, mi.profile_overrides); // If there is a move profile associated with it, override the parameters if (cur_move_profile) diff --git a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp index 5abebcbf4c..ddebc07044 100644 --- a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp @@ -106,10 +106,10 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer // Get Composite Profile std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); // Create data structures for checking for plan profile overrides auto flattened = ci.flatten(moveFilter); diff --git a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp index 2afe984c89..81788c8778 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -90,10 +90,10 @@ TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerContext& // Get Composite Profile const auto& ci = input_data_poly.as(); std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); + profile = getProfileString(ns_, profile, problem.composite_profile_remapping); auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, profile, *problem.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); assert(cur_composite_profile->longest_valid_segment_length > 0); InstructionPoly start_instruction;