From fcf5f7cbe5abc16528c4455965706b634ce4aa21 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 8 Jan 2024 14:13:45 -0600 Subject: [PATCH 01/10] Add optional namespace field to task nodes --- tesseract_task_composer/core/src/task_composer_graph.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tesseract_task_composer/core/src/task_composer_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp index 56b29da5c2..92538ff631 100644 --- a/tesseract_task_composer/core/src/task_composer_graph.cpp +++ b/tesseract_task_composer/core/src/task_composer_graph.cpp @@ -79,7 +79,11 @@ TaskComposerGraph::TaskComposerGraph(std::string name, if (YAML::Node cn = node_it->second["config"]) plugin_info.config = cn; - TaskComposerNode::UPtr task_node = plugin_factory.createTaskComposerNode(node_name, plugin_info); + std::string namespace_name = node_name; + if (YAML::Node ns = node_it->second["namespace"]) + namespace_name = ns.as(); + + TaskComposerNode::UPtr task_node = plugin_factory.createTaskComposerNode(namespace_name, plugin_info); if (task_node == nullptr) throw std::runtime_error("Task Composer Graph '" + name_ + "' failed to create node '" + node_name + "'"); From cad450fe6325420eacec7320c6eebb46a8b4d6c8 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 9 Jan 2024 17:02:20 -0600 Subject: [PATCH 02/10] Convert task node to use namespace in config --- .../core/task_composer_node.h | 9 +++++++++ .../core/src/task_composer_graph.cpp | 6 +----- .../core/src/task_composer_node.cpp | 11 ++++++++++- .../planning/nodes/motion_planner_task.hpp | 4 ++-- .../planning/src/nodes/check_input_task.cpp | 6 +++--- .../src/nodes/continuous_contact_check_task.cpp | 6 +++--- .../src/nodes/discrete_contact_check_task.cpp | 6 +++--- .../planning/src/nodes/fix_state_bounds_task.cpp | 6 +++--- .../planning/src/nodes/fix_state_collision_task.cpp | 6 +++--- .../nodes/iterative_spline_parameterization_task.cpp | 12 ++++++------ .../planning/src/nodes/min_length_task.cpp | 12 ++++++------ .../planning/src/nodes/profile_switch_task.cpp | 6 +++--- .../src/nodes/ruckig_trajectory_smoothing_task.cpp | 12 ++++++------ .../src/nodes/time_optimal_parameterization_task.cpp | 6 +++--- .../planning/src/nodes/upsample_trajectory_task.cpp | 6 +++--- .../taskflow/src/taskflow_task_composer_executor.cpp | 2 +- 16 files changed, 65 insertions(+), 51 deletions(-) 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_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp index 92538ff631..56b29da5c2 100644 --- a/tesseract_task_composer/core/src/task_composer_graph.cpp +++ b/tesseract_task_composer/core/src/task_composer_graph.cpp @@ -79,11 +79,7 @@ TaskComposerGraph::TaskComposerGraph(std::string name, if (YAML::Node cn = node_it->second["config"]) plugin_info.config = cn; - std::string namespace_name = node_name; - if (YAML::Node ns = node_it->second["namespace"]) - namespace_name = ns.as(); - - TaskComposerNode::UPtr task_node = plugin_factory.createTaskComposerNode(namespace_name, plugin_info); + TaskComposerNode::UPtr task_node = plugin_factory.createTaskComposerNode(node_name, plugin_info); if (task_node == nullptr) throw std::runtime_error("Task Composer Graph '" + name_ + "' failed to create node '" + node_name + "'"); diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 2128fc2f54..f9cf706b0d 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -51,6 +51,11 @@ TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, { try { + if (YAML::Node n = config["namespace"]) + ns_ = n.as(); + else + ns_ = name_; + if (YAML::Node n = config["conditional"]) conditional_ = n.as(); @@ -79,6 +84,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 +142,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..b1dbc78193 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,17 +108,17 @@ 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); // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(planner.getName(), ci.getProfile(), profile); + profiles->addProfile(planner.getNamespace(), ci.getProfile(), profile); auto flat = ci.flatten(&moveFilter); for (const auto& i : flat) profiles->addProfile( - planner.getName(), i.get().as().getProfile(), profile); + planner.getNamespace(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = profiles; 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; diff --git a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp index ee25e264ec..0d6cd49cb2 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp @@ -82,7 +82,7 @@ void TaskflowTaskComposerExecutor::removeFuture(const boost::uuids::uuid& uuid) TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNode& node, TaskComposerContext::Ptr context) { - auto taskflow = std::make_unique(node.getName()); + auto taskflow = std::make_unique(node.getNamespace()); if (node.getType() == TaskComposerNodeType::TASK) convertToTaskflow(static_cast(node), *context, *this, taskflow.get()); else if (node.getType() == TaskComposerNodeType::PIPELINE) From 88ef0b222cf3f6ead11d1fa404e4b5af63ab7f85 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 9 Jan 2024 17:08:36 -0600 Subject: [PATCH 03/10] Changed name to namespace for motion planner tasks --- .../tesseract_motion_planners/core/planner.h | 8 +++--- .../core/src/planner.cpp | 8 +++--- .../descartes/descartes_motion_planner.h | 2 +- .../impl/descartes_motion_planner.hpp | 10 ++++---- .../ompl/ompl_motion_planner.h | 2 +- .../ompl/src/ompl_motion_planner.cpp | 10 ++++---- .../simple/simple_motion_planner.h | 2 +- .../simple/src/interpolation.cpp | 4 +-- .../simple/src/simple_motion_planner.cpp | 17 ++++++------- .../trajopt/trajopt_motion_planner.h | 2 +- .../trajopt/src/trajopt_motion_planner.cpp | 24 +++++++++--------- .../trajopt_ifopt_motion_planner.h | 2 +- .../src/trajopt_ifopt_motion_planner.cpp | 25 ++++++++----------- .../planning/nodes/motion_planner_task.hpp | 2 +- 14 files changed, 57 insertions(+), 61 deletions(-) diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h index 128e6d96f5..5d35f99a81 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h @@ -37,7 +37,7 @@ class MotionPlanner using ConstPtr = std::shared_ptr; /** @brief Construct a basic planner */ MotionPlanner() = default; - MotionPlanner(std::string name); + MotionPlanner(std::string ns); virtual ~MotionPlanner() = default; MotionPlanner(const MotionPlanner&) = delete; MotionPlanner& operator=(const MotionPlanner&) = delete; @@ -45,10 +45,10 @@ class MotionPlanner MotionPlanner& operator=(MotionPlanner&&) = delete; /** - * @brief Get the name of this planner + * @brief Get the namespace of this planner * @details This is also used as the namespace for the profiles in the profile dictionary */ - const std::string& getName() const; + const std::string& getNamespace() const; /** * @brief Solve the planner request problem @@ -79,7 +79,7 @@ class MotionPlanner bool format_result_as_input); protected: - std::string name_; + std::string ns_; }; } // namespace tesseract_planning #endif // TESSERACT_PLANNING_PLANNER_H diff --git a/tesseract_motion_planners/core/src/planner.cpp b/tesseract_motion_planners/core/src/planner.cpp index b401af7e48..0e2aa823ef 100644 --- a/tesseract_motion_planners/core/src/planner.cpp +++ b/tesseract_motion_planners/core/src/planner.cpp @@ -28,13 +28,13 @@ namespace tesseract_planning { -MotionPlanner::MotionPlanner(std::string name) : name_(std::move(name)) +MotionPlanner::MotionPlanner(std::string ns) : ns_(std::move(ns)) { - if (name_.empty()) - throw std::runtime_error("MotionPlanner name is empty!"); + if (ns_.empty()) + throw std::runtime_error("MotionPlanner namespace is empty!"); } -const std::string& MotionPlanner::getName() const { return name_; } +const std::string& MotionPlanner::getNamespace() const { return ns_; } bool MotionPlanner::checkRequest(const PlannerRequest& request) { diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h index e68537204b..40f64773a7 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h @@ -19,7 +19,7 @@ class DescartesMotionPlanner : public MotionPlanner { public: /** @brief Construct a basic planner */ - DescartesMotionPlanner(std::string name); + DescartesMotionPlanner(std::string ns); ~DescartesMotionPlanner() override = default; DescartesMotionPlanner(const DescartesMotionPlanner&) = delete; DescartesMotionPlanner& operator=(const DescartesMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index 7b9654ca89..82cfe88bb4 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -55,7 +55,7 @@ constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid soluti namespace tesseract_planning { template -DescartesMotionPlanner::DescartesMotionPlanner(std::string name) : MotionPlanner(std::move(name)) // NOLINT +DescartesMotionPlanner::DescartesMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) // NOLINT { } @@ -213,7 +213,7 @@ void DescartesMotionPlanner::clear() template MotionPlanner::Ptr DescartesMotionPlanner::clone() const { - return std::make_shared>(name_); + return std::make_shared>(ns_); } template @@ -284,10 +284,10 @@ DescartesMotionPlanner::createProblem(const PlannerRequest& request) // Get Plan Profile std::string profile = plan_instruction.getProfile(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); + profile = getProfileString(ns_, profile, request.plan_profile_remapping); auto cur_plan_profile = getProfile>( - name_, profile, *request.profiles, std::make_shared>()); - // cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, + ns_, profile, *request.profiles, std::make_shared>()); + // cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, // plan_instruction.profile_overrides); if (!cur_plan_profile) throw std::runtime_error("DescartesMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index 2f09f1d3e4..56358d269b 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -56,7 +56,7 @@ class OMPLMotionPlanner : public MotionPlanner { public: /** @brief Construct a planner */ - OMPLMotionPlanner(std::string name); + OMPLMotionPlanner(std::string ns); /** * @brief Sets up the OMPL problem then solves. It is intended to simplify setting up diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 74dc5539c3..58cf9e7866 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -89,7 +89,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, } /** @brief Construct a basic planner */ -OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} +OMPLMotionPlanner::OMPLMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} bool OMPLMotionPlanner::terminate() { @@ -307,7 +307,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const void OMPLMotionPlanner::clear() { parallel_plan_ = nullptr; } -MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(name_); } +MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(ns_); } OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& request, const tesseract_common::ManipulatorInfo& composite_mi, @@ -322,10 +322,10 @@ OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& requ // Get Plan Profile std::string profile = end_instruction.getProfile(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); + profile = getProfileString(ns_, profile, request.plan_profile_remapping); auto cur_plan_profile = - getProfile(name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, end_instruction.getProfileOverrides()); + getProfile(ns_, profile, *request.profiles, std::make_shared()); + cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, end_instruction.getProfileOverrides()); if (!cur_plan_profile) throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h index 37cb120754..49d494f48b 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h @@ -52,7 +52,7 @@ class SimpleMotionPlanner : public MotionPlanner using ConstPtr = std::shared_ptr; /** @brief Construct a basic planner */ - SimpleMotionPlanner(std::string name); + SimpleMotionPlanner(std::string ns); ~SimpleMotionPlanner() override = default; SimpleMotionPlanner(const SimpleMotionPlanner&) = delete; SimpleMotionPlanner& operator=(const SimpleMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 480ca0b57c..199373c3ee 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -1302,11 +1302,11 @@ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& ins // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(planner.getName(), instructions.getProfile(), profile); + profiles->addProfile(planner.getNamespace(), instructions.getProfile(), profile); auto flat = instructions.flatten(&moveFilter); for (const auto& i : flat) profiles->addProfile( - planner.getName(), i.get().as().getProfile(), profile); + planner.getNamespace(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = profiles; diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index fab9fcb73e..a0e0085fec 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -46,7 +46,7 @@ constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" }; namespace tesseract_planning { -SimpleMotionPlanner::SimpleMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} +SimpleMotionPlanner::SimpleMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} bool SimpleMotionPlanner::terminate() { @@ -56,7 +56,7 @@ bool SimpleMotionPlanner::terminate() void SimpleMotionPlanner::clear() {} -MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared(name_); } +MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared(ns_); } PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const { @@ -196,18 +196,17 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp SimplePlannerPlanProfile::ConstPtr plan_profile; if (base_instruction.getPathProfile().empty()) { - std::string profile = getProfileString(name_, base_instruction.getProfile(), request.plan_profile_remapping); + std::string profile = getProfileString(ns_, base_instruction.getProfile(), request.plan_profile_remapping); plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); + ns_, profile, *request.profiles, std::make_shared()); + plan_profile = applyProfileOverrides(ns_, profile, plan_profile, base_instruction.getProfileOverrides()); } else { - std::string profile = - getProfileString(name_, base_instruction.getPathProfile(), request.plan_profile_remapping); + std::string profile = getProfileString(ns_, base_instruction.getPathProfile(), request.plan_profile_remapping); plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); + ns_, profile, *request.profiles, std::make_shared()); + plan_profile = applyProfileOverrides(ns_, profile, plan_profile, base_instruction.getProfileOverrides()); } if (!plan_profile) diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h index 92520b2423..c3cf6c6b1f 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h @@ -40,7 +40,7 @@ class TrajOptMotionPlanner : public MotionPlanner { public: /** @brief Construct a basic planner */ - TrajOptMotionPlanner(std::string name); + TrajOptMotionPlanner(std::string ns); ~TrajOptMotionPlanner() override = default; TrajOptMotionPlanner(const TrajOptMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 13905c9d6d..133a052d72 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -52,7 +52,7 @@ using namespace trajopt; namespace tesseract_planning { -TrajOptMotionPlanner::TrajOptMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} +TrajOptMotionPlanner::TrajOptMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} bool TrajOptMotionPlanner::terminate() { @@ -62,7 +62,7 @@ bool TrajOptMotionPlanner::terminate() void TrajOptMotionPlanner::clear() {} -MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared(name_); } +MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared(ns_); } PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const { @@ -212,10 +212,10 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const // Apply Solver parameters std::string profile = request.instructions.getProfile(); ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); + profile = getProfileString(ns_, profile, request.plan_profile_remapping); TrajOptSolverProfile::ConstPtr solver_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides); + ns_, profile, *request.profiles, std::make_shared()); + solver_profile = applyProfileOverrides(ns_, profile, solver_profile, profile_overrides); if (!solver_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -250,10 +250,10 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); - TrajOptPlanProfile::ConstPtr cur_plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); + std::string profile = getProfileString(ns_, move_instruction.getProfile(), request.plan_profile_remapping); + TrajOptPlanProfile::ConstPtr cur_plan_profile = + getProfile(ns_, profile, *request.profiles, std::make_shared()); + cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); if (!cur_plan_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -333,11 +333,11 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const for (long i = 0; i < pci->basic_info.n_steps; ++i) pci->init_info.data.row(i) = seed_states[static_cast(i)]; - profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); + profile = getProfileString(ns_, request.instructions.getProfile(), request.composite_profile_remapping); TrajOptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); + ns_, profile, *request.profiles, std::make_shared()); cur_composite_profile = - applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); + applyProfileOverrides(ns_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); if (!cur_composite_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h index 58c09d80a5..b12ca3f1a3 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h @@ -41,7 +41,7 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner { public: /** @brief Construct a basic planner */ - TrajOptIfoptMotionPlanner(std::string name); + TrajOptIfoptMotionPlanner(std::string ns); ~TrajOptIfoptMotionPlanner() override = default; TrajOptIfoptMotionPlanner(const TrajOptIfoptMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 302c0daa76..906ec05803 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -50,7 +50,7 @@ using namespace trajopt_ifopt; namespace tesseract_planning { -TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} +TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} bool TrajOptIfoptMotionPlanner::terminate() { @@ -58,10 +58,7 @@ bool TrajOptIfoptMotionPlanner::terminate() return false; } -MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const -{ - return std::make_shared(name_); -} +MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const { return std::make_shared(ns_); } PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const { @@ -218,10 +215,10 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Apply Solver parameters std::string profile = request.instructions.getProfile(); ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); + profile = getProfileString(ns_, profile, request.plan_profile_remapping); TrajOptIfoptSolverProfile::ConstPtr solver_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides); + ns_, profile, *request.profiles, std::make_shared()); + solver_profile = applyProfileOverrides(ns_, profile, solver_profile, profile_overrides); if (!solver_profile) throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile"); @@ -256,10 +253,10 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); + std::string profile = getProfileString(ns_, move_instruction.getProfile(), request.plan_profile_remapping); TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); + ns_, profile, *request.profiles, std::make_shared()); + cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); if (!cur_plan_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -344,11 +341,11 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // ---------------- // Translate TCL for CompositeInstructions // ---------------- - profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); + profile = getProfileString(ns_, request.instructions.getProfile(), request.composite_profile_remapping); TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); + ns_, profile, *request.profiles, std::make_shared()); cur_composite_profile = - applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); + applyProfileOverrides(ns_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); if (!cur_composite_profile) throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile"); 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 654f693eaa..85e6b173ad 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 @@ -173,7 +173,7 @@ class MotionPlannerTask : public TaskComposerTask } CONSOLE_BRIDGE_logInform("%s motion planning failed (%s) for process input: %s", - planner_->getName().c_str(), + planner_->getNamespace().c_str(), response.message.c_str(), instructions.getDescription().c_str()); From d54c6957b4b1e37664fe046fc45d70cc04d6caaa Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 10 Jan 2024 09:16:04 -0600 Subject: [PATCH 04/10] Default namespace to task name --- tesseract_task_composer/core/src/task_composer_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index f9cf706b0d..4458b5a0d2 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_)) From 19c36411262e0768fdd9f082fe3478a3374ae479 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 10 Jan 2024 11:38:35 -0600 Subject: [PATCH 05/10] Simplify setting ns_ to a single line --- tesseract_task_composer/core/src/task_composer_node.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 4458b5a0d2..ef590a0485 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -52,10 +52,7 @@ TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, { try { - if (YAML::Node n = config["namespace"]) - ns_ = n.as(); - else - ns_ = name_; + ns_ = config["namespace"] ? config["namespace"].as() : name_; if (YAML::Node n = config["conditional"]) conditional_ = n.as(); From 7fdff4736a6c9912b626d28980d82901c84687ad Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 10 Jan 2024 11:39:01 -0600 Subject: [PATCH 06/10] Correct naming of taskflow instances to use name, not namespace --- .../taskflow/src/taskflow_task_composer_executor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp index 0d6cd49cb2..ee25e264ec 100644 --- a/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp +++ b/tesseract_task_composer/taskflow/src/taskflow_task_composer_executor.cpp @@ -82,7 +82,7 @@ void TaskflowTaskComposerExecutor::removeFuture(const boost::uuids::uuid& uuid) TaskComposerFuture::UPtr TaskflowTaskComposerExecutor::run(const TaskComposerNode& node, TaskComposerContext::Ptr context) { - auto taskflow = std::make_unique(node.getNamespace()); + auto taskflow = std::make_unique(node.getName()); if (node.getType() == TaskComposerNodeType::TASK) convertToTaskflow(static_cast(node), *context, *this, taskflow.get()); else if (node.getType() == TaskComposerNodeType::PIPELINE) From ee1aac44411a432054a8cc3a6a702c18e070de65 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 10 Jan 2024 15:31:54 -0600 Subject: [PATCH 07/10] Revert "Changed name to namespace for motion planner tasks" This reverts commit 88ef0b222cf3f6ead11d1fa404e4b5af63ab7f85. --- .../tesseract_motion_planners/core/planner.h | 8 +++--- .../core/src/planner.cpp | 8 +++--- .../descartes/descartes_motion_planner.h | 2 +- .../impl/descartes_motion_planner.hpp | 10 ++++---- .../ompl/ompl_motion_planner.h | 2 +- .../ompl/src/ompl_motion_planner.cpp | 10 ++++---- .../simple/simple_motion_planner.h | 2 +- .../simple/src/interpolation.cpp | 4 +-- .../simple/src/simple_motion_planner.cpp | 17 +++++++------ .../trajopt/trajopt_motion_planner.h | 2 +- .../trajopt/src/trajopt_motion_planner.cpp | 24 +++++++++--------- .../trajopt_ifopt_motion_planner.h | 2 +- .../src/trajopt_ifopt_motion_planner.cpp | 25 +++++++++++-------- .../planning/nodes/motion_planner_task.hpp | 2 +- 14 files changed, 61 insertions(+), 57 deletions(-) diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h index 5d35f99a81..128e6d96f5 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h @@ -37,7 +37,7 @@ class MotionPlanner using ConstPtr = std::shared_ptr; /** @brief Construct a basic planner */ MotionPlanner() = default; - MotionPlanner(std::string ns); + MotionPlanner(std::string name); virtual ~MotionPlanner() = default; MotionPlanner(const MotionPlanner&) = delete; MotionPlanner& operator=(const MotionPlanner&) = delete; @@ -45,10 +45,10 @@ class MotionPlanner MotionPlanner& operator=(MotionPlanner&&) = delete; /** - * @brief Get the namespace of this planner + * @brief Get the name of this planner * @details This is also used as the namespace for the profiles in the profile dictionary */ - const std::string& getNamespace() const; + const std::string& getName() const; /** * @brief Solve the planner request problem @@ -79,7 +79,7 @@ class MotionPlanner bool format_result_as_input); protected: - std::string ns_; + std::string name_; }; } // namespace tesseract_planning #endif // TESSERACT_PLANNING_PLANNER_H diff --git a/tesseract_motion_planners/core/src/planner.cpp b/tesseract_motion_planners/core/src/planner.cpp index 0e2aa823ef..b401af7e48 100644 --- a/tesseract_motion_planners/core/src/planner.cpp +++ b/tesseract_motion_planners/core/src/planner.cpp @@ -28,13 +28,13 @@ namespace tesseract_planning { -MotionPlanner::MotionPlanner(std::string ns) : ns_(std::move(ns)) +MotionPlanner::MotionPlanner(std::string name) : name_(std::move(name)) { - if (ns_.empty()) - throw std::runtime_error("MotionPlanner namespace is empty!"); + if (name_.empty()) + throw std::runtime_error("MotionPlanner name is empty!"); } -const std::string& MotionPlanner::getNamespace() const { return ns_; } +const std::string& MotionPlanner::getName() const { return name_; } bool MotionPlanner::checkRequest(const PlannerRequest& request) { diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h index 40f64773a7..e68537204b 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h @@ -19,7 +19,7 @@ class DescartesMotionPlanner : public MotionPlanner { public: /** @brief Construct a basic planner */ - DescartesMotionPlanner(std::string ns); + DescartesMotionPlanner(std::string name); ~DescartesMotionPlanner() override = default; DescartesMotionPlanner(const DescartesMotionPlanner&) = delete; DescartesMotionPlanner& operator=(const DescartesMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index 82cfe88bb4..7b9654ca89 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -55,7 +55,7 @@ constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid soluti namespace tesseract_planning { template -DescartesMotionPlanner::DescartesMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) // NOLINT +DescartesMotionPlanner::DescartesMotionPlanner(std::string name) : MotionPlanner(std::move(name)) // NOLINT { } @@ -213,7 +213,7 @@ void DescartesMotionPlanner::clear() template MotionPlanner::Ptr DescartesMotionPlanner::clone() const { - return std::make_shared>(ns_); + return std::make_shared>(name_); } template @@ -284,10 +284,10 @@ DescartesMotionPlanner::createProblem(const PlannerRequest& request) // Get Plan Profile std::string profile = plan_instruction.getProfile(); - profile = getProfileString(ns_, profile, request.plan_profile_remapping); + profile = getProfileString(name_, profile, request.plan_profile_remapping); auto cur_plan_profile = getProfile>( - ns_, profile, *request.profiles, std::make_shared>()); - // cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, + name_, profile, *request.profiles, std::make_shared>()); + // cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, // plan_instruction.profile_overrides); if (!cur_plan_profile) throw std::runtime_error("DescartesMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index 56358d269b..2f09f1d3e4 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -56,7 +56,7 @@ class OMPLMotionPlanner : public MotionPlanner { public: /** @brief Construct a planner */ - OMPLMotionPlanner(std::string ns); + OMPLMotionPlanner(std::string name); /** * @brief Sets up the OMPL problem then solves. It is intended to simplify setting up diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 58cf9e7866..74dc5539c3 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -89,7 +89,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, } /** @brief Construct a basic planner */ -OMPLMotionPlanner::OMPLMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} +OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} bool OMPLMotionPlanner::terminate() { @@ -307,7 +307,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const void OMPLMotionPlanner::clear() { parallel_plan_ = nullptr; } -MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(ns_); } +MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(name_); } OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& request, const tesseract_common::ManipulatorInfo& composite_mi, @@ -322,10 +322,10 @@ OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& requ // Get Plan Profile std::string profile = end_instruction.getProfile(); - profile = getProfileString(ns_, profile, request.plan_profile_remapping); + profile = getProfileString(name_, profile, request.plan_profile_remapping); auto cur_plan_profile = - getProfile(ns_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, end_instruction.getProfileOverrides()); + getProfile(name_, profile, *request.profiles, std::make_shared()); + cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, end_instruction.getProfileOverrides()); if (!cur_plan_profile) throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h index 49d494f48b..37cb120754 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h @@ -52,7 +52,7 @@ class SimpleMotionPlanner : public MotionPlanner using ConstPtr = std::shared_ptr; /** @brief Construct a basic planner */ - SimpleMotionPlanner(std::string ns); + SimpleMotionPlanner(std::string name); ~SimpleMotionPlanner() override = default; SimpleMotionPlanner(const SimpleMotionPlanner&) = delete; SimpleMotionPlanner& operator=(const SimpleMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 199373c3ee..480ca0b57c 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -1302,11 +1302,11 @@ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& ins // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(planner.getNamespace(), instructions.getProfile(), profile); + profiles->addProfile(planner.getName(), instructions.getProfile(), profile); auto flat = instructions.flatten(&moveFilter); for (const auto& i : flat) profiles->addProfile( - planner.getNamespace(), i.get().as().getProfile(), profile); + planner.getName(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = profiles; diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index a0e0085fec..fab9fcb73e 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -46,7 +46,7 @@ constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" }; namespace tesseract_planning { -SimpleMotionPlanner::SimpleMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} +SimpleMotionPlanner::SimpleMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} bool SimpleMotionPlanner::terminate() { @@ -56,7 +56,7 @@ bool SimpleMotionPlanner::terminate() void SimpleMotionPlanner::clear() {} -MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared(ns_); } +MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared(name_); } PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const { @@ -196,17 +196,18 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp SimplePlannerPlanProfile::ConstPtr plan_profile; if (base_instruction.getPathProfile().empty()) { - std::string profile = getProfileString(ns_, base_instruction.getProfile(), request.plan_profile_remapping); + std::string profile = getProfileString(name_, base_instruction.getProfile(), request.plan_profile_remapping); plan_profile = getProfile( - ns_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(ns_, profile, plan_profile, base_instruction.getProfileOverrides()); + name_, profile, *request.profiles, std::make_shared()); + plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); } else { - std::string profile = getProfileString(ns_, base_instruction.getPathProfile(), request.plan_profile_remapping); + std::string profile = + getProfileString(name_, base_instruction.getPathProfile(), request.plan_profile_remapping); plan_profile = getProfile( - ns_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(ns_, profile, plan_profile, base_instruction.getProfileOverrides()); + name_, profile, *request.profiles, std::make_shared()); + plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); } if (!plan_profile) diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h index c3cf6c6b1f..92520b2423 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_motion_planner.h @@ -40,7 +40,7 @@ class TrajOptMotionPlanner : public MotionPlanner { public: /** @brief Construct a basic planner */ - TrajOptMotionPlanner(std::string ns); + TrajOptMotionPlanner(std::string name); ~TrajOptMotionPlanner() override = default; TrajOptMotionPlanner(const TrajOptMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 133a052d72..13905c9d6d 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -52,7 +52,7 @@ using namespace trajopt; namespace tesseract_planning { -TrajOptMotionPlanner::TrajOptMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} +TrajOptMotionPlanner::TrajOptMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} bool TrajOptMotionPlanner::terminate() { @@ -62,7 +62,7 @@ bool TrajOptMotionPlanner::terminate() void TrajOptMotionPlanner::clear() {} -MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared(ns_); } +MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared(name_); } PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const { @@ -212,10 +212,10 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const // Apply Solver parameters std::string profile = request.instructions.getProfile(); ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(ns_, profile, request.plan_profile_remapping); + profile = getProfileString(name_, profile, request.plan_profile_remapping); TrajOptSolverProfile::ConstPtr solver_profile = getProfile( - ns_, profile, *request.profiles, std::make_shared()); - solver_profile = applyProfileOverrides(ns_, profile, solver_profile, profile_overrides); + name_, profile, *request.profiles, std::make_shared()); + solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides); if (!solver_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -250,10 +250,10 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(ns_, move_instruction.getProfile(), request.plan_profile_remapping); - TrajOptPlanProfile::ConstPtr cur_plan_profile = - getProfile(ns_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); + std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); + TrajOptPlanProfile::ConstPtr cur_plan_profile = getProfile( + name_, profile, *request.profiles, std::make_shared()); + cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); if (!cur_plan_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -333,11 +333,11 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const for (long i = 0; i < pci->basic_info.n_steps; ++i) pci->init_info.data.row(i) = seed_states[static_cast(i)]; - profile = getProfileString(ns_, request.instructions.getProfile(), request.composite_profile_remapping); + profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); TrajOptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - ns_, profile, *request.profiles, std::make_shared()); + name_, profile, *request.profiles, std::make_shared()); cur_composite_profile = - applyProfileOverrides(ns_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); + applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); if (!cur_composite_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h index b12ca3f1a3..58c09d80a5 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h @@ -41,7 +41,7 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner { public: /** @brief Construct a basic planner */ - TrajOptIfoptMotionPlanner(std::string ns); + TrajOptIfoptMotionPlanner(std::string name); ~TrajOptIfoptMotionPlanner() override = default; TrajOptIfoptMotionPlanner(const TrajOptIfoptMotionPlanner&) = delete; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 906ec05803..302c0daa76 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -50,7 +50,7 @@ using namespace trajopt_ifopt; namespace tesseract_planning { -TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner(std::string ns) : MotionPlanner(std::move(ns)) {} +TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner(std::string name) : MotionPlanner(std::move(name)) {} bool TrajOptIfoptMotionPlanner::terminate() { @@ -58,7 +58,10 @@ bool TrajOptIfoptMotionPlanner::terminate() return false; } -MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const { return std::make_shared(ns_); } +MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const +{ + return std::make_shared(name_); +} PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const { @@ -215,10 +218,10 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Apply Solver parameters std::string profile = request.instructions.getProfile(); ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(ns_, profile, request.plan_profile_remapping); + profile = getProfileString(name_, profile, request.plan_profile_remapping); TrajOptIfoptSolverProfile::ConstPtr solver_profile = getProfile( - ns_, profile, *request.profiles, std::make_shared()); - solver_profile = applyProfileOverrides(ns_, profile, solver_profile, profile_overrides); + name_, profile, *request.profiles, std::make_shared()); + solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides); if (!solver_profile) throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile"); @@ -253,10 +256,10 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(ns_, move_instruction.getProfile(), request.plan_profile_remapping); + std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile( - ns_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(ns_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); + name_, profile, *request.profiles, std::make_shared()); + cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); if (!cur_plan_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -341,11 +344,11 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // ---------------- // Translate TCL for CompositeInstructions // ---------------- - profile = getProfileString(ns_, request.instructions.getProfile(), request.composite_profile_remapping); + profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - ns_, profile, *request.profiles, std::make_shared()); + name_, profile, *request.profiles, std::make_shared()); cur_composite_profile = - applyProfileOverrides(ns_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); + applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); if (!cur_composite_profile) throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile"); 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 85e6b173ad..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 @@ -173,7 +173,7 @@ class MotionPlannerTask : public TaskComposerTask } CONSOLE_BRIDGE_logInform("%s motion planning failed (%s) for process input: %s", - planner_->getNamespace().c_str(), + planner_->getName().c_str(), response.message.c_str(), instructions.getDescription().c_str()); From 386b9aa9b60abb423d937019360683484051e1f7 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 10 Jan 2024 15:41:24 -0600 Subject: [PATCH 08/10] Finsih reverting ns_ to name_ for `MotionPlanner` --- .../planning/src/nodes/min_length_task.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 b1dbc78193..367d5779f4 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -114,11 +114,11 @@ TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerContext& context, // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(planner.getNamespace(), ci.getProfile(), profile); + profiles->addProfile(planner.getName(), ci.getProfile(), profile); auto flat = ci.flatten(&moveFilter); for (const auto& i : flat) profiles->addProfile( - planner.getNamespace(), i.get().as().getProfile(), profile); + planner.getName(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = profiles; From 8d435129510f53ec8f96d5a3d9f93733c272c481 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 10 Jan 2024 17:12:03 -0600 Subject: [PATCH 09/10] Adding namespace field to README --- tesseract_task_composer/README.rst | 123 ++++++++++++++++++++++++++++- 1 file changed, 120 insertions(+), 3 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] From 12c79093d30237086dba076d0fe4a11275e2b8ee Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Thu, 11 Jan 2024 08:51:16 -0600 Subject: [PATCH 10/10] Fix clang-tidy error --- tesseract_task_composer/core/src/task_composer_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index ef590a0485..62b3ed3ba2 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -52,7 +52,7 @@ TaskComposerNode::TaskComposerNode(std::string name, TaskComposerNodeType type, { try { - ns_ = config["namespace"] ? config["namespace"].as() : name_; + ns_ = config["namespace"].IsDefined() ? config["namespace"].as() : name_; if (YAML::Node n = config["conditional"]) conditional_ = n.as();