From d3dde9c0cb35f8a2d1bafcefe9c42dcca5548cf5 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Tue, 22 Oct 2024 13:04:21 -0400
Subject: [PATCH 01/15] [refac] replace jump_threshold with precision

---
 src/create_mtc_cartesian_path.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/create_mtc_cartesian_path.cpp b/src/create_mtc_cartesian_path.cpp
index 5a621c3..0267279 100644
--- a/src/create_mtc_cartesian_path.cpp
+++ b/src/create_mtc_cartesian_path.cpp
@@ -18,23 +18,24 @@ BT::NodeStatus CreateMTCCartesianPath::tick()
   double max_velocity_scaling_factor;
   double max_acceleration_scaling_factor;
   double step_size;
-  double jump_treshold;
   double min_fraction;
+  moveit::core::CartesianPrecision precision;
 
   if(!getInput("max_velocity_scaling_factor", max_velocity_scaling_factor) ||
      !getInput("max_acceleration_scaling_factor", max_acceleration_scaling_factor) ||
      !getInput("step_size", step_size) ||
      !getInput("min_fraction", min_fraction) ||
-     !getInput("jump_threshold", jump_treshold))
+     !getInput("precision", precision))
     return NodeStatus::FAILURE;
 
-  //build solver
+  // Build solver
   auto solver = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
   solver->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
   solver->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
   solver->setStepSize(step_size);
-  solver->setJumpThreshold(jump_treshold);
   solver->setMinFraction(min_fraction);
+  solver->setPrecision(precision);
+
   // Upcast to base class
   moveit::task_constructor::solvers::PlannerInterfacePtr base_solver = solver;
 
@@ -47,9 +48,9 @@ BT::PortsList CreateMTCCartesianPath::providedPorts()
   return {
     BT::InputPort<double>("max_velocity_scaling_factor", 0.1, "scale down max velocity by this factor"),
     BT::InputPort<double>("max_acceleration_scaling_factor", 0.1, "scale down max acceleration by this factor"),
-    BT::InputPort<double>("step_size", 0.01, "step size between consecutive waypoints"),
     BT::InputPort<double>("min_fraction", 1.0, "fraction of motion required for success"),
-    BT::InputPort<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"),
+    BT::InputPort<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision({ .translational = 0.001, .rotational = 0.01, .max_resolution = 1e-5 }), "linear and rotational precision"),
+    BT::InputPort<double>("step_size", 0.01, "step size between consecutive waypoints"),
     BT::OutputPort<moveit::task_constructor::solvers::PlannerInterfacePtr>("solver", "{solver}", "Planner interface using pipeline motion solver"),
   };
 }

From 2f957dcd57e3f0f88eba217c0129561004ad8e54 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Tue, 5 Nov 2024 17:14:23 -0500
Subject: [PATCH 02/15] Make MTC planning preemptable (#26)

We must stop the planning thread fast when the halt() method is invoked.
https://www.behaviortree.dev/docs/guides/asynchronous_nodes#the-problem-with-multi-threading
---
 include/behaviortree_mtc/plan_mtc_task.h |   7 +
 src/plan_mtc_task.cpp                    |  49 +++++-
 tests/CMakeLists.txt                     |   8 +
 tests/gtest_plan_mtc_task.cpp            |  57 +++++++
 tests/models.cpp                         |  25 +++
 tests/models.h                           |  15 ++
 tests/stage_mockups.cpp                  | 163 ++++++++++++++++++
 tests/stage_mockups.h                    | 201 +++++++++++++++++++++++
 8 files changed, 521 insertions(+), 4 deletions(-)
 create mode 100644 tests/gtest_plan_mtc_task.cpp
 create mode 100644 tests/models.cpp
 create mode 100644 tests/models.h
 create mode 100644 tests/stage_mockups.cpp
 create mode 100644 tests/stage_mockups.h

diff --git a/include/behaviortree_mtc/plan_mtc_task.h b/include/behaviortree_mtc/plan_mtc_task.h
index da37688..1879eec 100644
--- a/include/behaviortree_mtc/plan_mtc_task.h
+++ b/include/behaviortree_mtc/plan_mtc_task.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <behaviortree_cpp/bt_factory.h>
+#include <moveit/task_constructor/task.h>
 
 namespace BT {
 namespace MTC {
@@ -11,7 +12,13 @@ class PlanMTCTask : public BT::ThreadedAction
   PlanMTCTask(const std::string& name, const BT::NodeConfig& config);
 
   BT::NodeStatus tick() override;
+  void halt() override;
   static BT::PortsList providedPorts();
+
+private:
+  moveit::task_constructor::TaskPtr task_;
+
+  std::mutex task_mutex_;
 };
 
 }  // namespace MTC
diff --git a/src/plan_mtc_task.cpp b/src/plan_mtc_task.cpp
index 3d38db1..2b7f4e8 100644
--- a/src/plan_mtc_task.cpp
+++ b/src/plan_mtc_task.cpp
@@ -1,7 +1,5 @@
 #include <behaviortree_mtc/plan_mtc_task.h>
 
-#include <moveit/task_constructor/task.h>
-
 namespace BT {
 namespace MTC {
 
@@ -11,21 +9,64 @@ PlanMTCTask::PlanMTCTask(const std::string& name, const BT::NodeConfig& config)
 
 BT::NodeStatus PlanMTCTask::tick()
 {
+  // Inputs
   const size_t max_solutions = getInput<size_t>("max_solutions").value();
   if(auto any_ptr = getLockedPortContent("task"))
   {
     if(auto* task_ptr = any_ptr->castPtr<moveit::task_constructor::TaskPtr>())
     {
       auto& task = *task_ptr;
-      if(task->plan(max_solutions))
+
+      // copy task internally
       {
-        return NodeStatus::SUCCESS;
+        std::unique_lock<std::mutex> lock{ task_mutex_ };
+        task_ = task;
       }
     }
   }
+
+  if(task_)
+  {
+    moveit::core::MoveItErrorCode ec;
+    try
+    {
+      task_->resetPreemptRequest();  // should not be needed
+      ec = task_->plan(max_solutions);
+      std::cout << ec.toString(ec) << std::endl;
+    }
+    catch(moveit::task_constructor::InitStageException e)
+    {
+      std::cout << e << std::endl;
+      return NodeStatus::FAILURE;
+    }
+
+    {
+      std::unique_lock<std::mutex> lock{ task_mutex_ };
+      task_ = nullptr;
+    }
+
+    if(ec.val == ec.SUCCESS)
+      return NodeStatus::SUCCESS;
+  }
+
   return NodeStatus::FAILURE;
 }
 
+void PlanMTCTask::halt()
+{
+  {
+    std::unique_lock<std::mutex> lock{ task_mutex_ };
+
+    if(task_)
+    {
+      task_->preempt();
+    }
+  }
+
+  // halt base class
+  ThreadedAction::halt();
+}
+
 BT::PortsList PlanMTCTask::providedPorts()
 {
   return {
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 325ec91..c654fb1 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -4,15 +4,23 @@
 set(BT_MTC_TESTS
   gtest_mtc_properties.cpp
   gtest_mtc_modify_planning_scene.cpp
+  gtest_plan_mtc_task.cpp
 )
 
 set(TEST_DEPENDECIES
+    gtest_utils
     ${PROJECT_NAME}
 )
 
 if(CATKIN_ENABLE_TESTING)
     find_package(rostest REQUIRED)
 
+
+
+
+    add_library(gtest_utils models.cpp stage_mockups.cpp)
+    target_link_libraries(gtest_utils ${PROJECT_NAME})
+
     catkin_add_gtest(${PROJECT_NAME}_test ${BT_MTC_TESTS})
     target_link_libraries(${PROJECT_NAME}_test
         ${TEST_DEPENDECIES}
diff --git a/tests/gtest_plan_mtc_task.cpp b/tests/gtest_plan_mtc_task.cpp
new file mode 100644
index 0000000..c9f9734
--- /dev/null
+++ b/tests/gtest_plan_mtc_task.cpp
@@ -0,0 +1,57 @@
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <behaviortree_cpp/bt_factory.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include "stage_mockups.h"
+#include "models.h"
+
+using namespace BT;
+using namespace BT::MTC;
+
+TEST(PlanMTCTask, Preempt)
+{
+  BT::BehaviorTreeFactory factory;
+
+  // clang-format off
+
+  static const char* xml_text = R"(
+
+   <root BTCPP_format="4" >
+       <BehaviorTree ID="MainTree">
+           <Sequence>
+               <Script code="force_failure := false"/>
+
+               <ReactiveSequence name="root">
+
+                   <AlwaysSuccess _failureIf="force_failure==true" _onSuccess="force_failure=true"/>
+                   <PlanMTCTask task="{mtc_task}" max_solutions="1"/>
+         </ReactiveSequence>
+
+       </Sequence>
+
+     </BehaviorTree>
+   </root>)";
+
+  // clang-format off
+
+  factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
+
+
+  // setup mtc task to be preempted
+  moveit::task_constructor::TaskPtr task = std::make_shared<moveit::task_constructor::Task>();
+  task->setRobotModel(getModel());
+
+  auto timeout = std::chrono::milliseconds(10);
+  task->add(std::make_unique<moveit::task_constructor::GeneratorMockup>(moveit::task_constructor::PredefinedCosts::constant(0.0)));
+  task->add(std::make_unique<moveit::task_constructor::TimedForwardMockup>(timeout));
+
+  // setup blackbaord
+  auto bb = BT::Blackboard::create();
+  bb->set("mtc_task", task);
+
+
+  // test
+  auto tree = factory.createTreeFromText(xml_text, bb);
+  EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING);
+  EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::FAILURE);
+}
diff --git a/tests/models.cpp b/tests/models.cpp
new file mode 100644
index 0000000..7283fbf
--- /dev/null
+++ b/tests/models.cpp
@@ -0,0 +1,25 @@
+#include "models.h"
+#include <moveit/utils/robot_model_test_utils.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
+using namespace moveit::core;
+
+RobotModelPtr getModel()
+{
+  // suppress RobotModel errors and warnings
+  ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME ".moveit_core.robot_model", ros::console::levels::Fatal);
+
+  // create dummy robot model
+  moveit::core::RobotModelBuilder builder("robot", "base");
+  builder.addChain("base->link1->link2->tip", "continuous");
+  builder.addGroupChain("base", "link2", "group");
+  builder.addGroupChain("link2", "tip", "eef_group");
+  builder.addEndEffector("eef", "link2", "group", "eef_group");
+  return builder.build();
+}
+
+moveit::core::RobotModelPtr loadModel()
+{
+  static robot_model_loader::RobotModelLoader loader;
+  return loader.getModel();
+}
diff --git a/tests/models.h b/tests/models.h
new file mode 100644
index 0000000..11cfcdb
--- /dev/null
+++ b/tests/models.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <moveit/macros/class_forward.h>
+
+namespace moveit {
+namespace core {
+MOVEIT_CLASS_FORWARD(RobotModel);
+}
+}  // namespace moveit
+
+// get a hard-coded model
+moveit::core::RobotModelPtr getModel();
+
+// load a model from robot_description
+moveit::core::RobotModelPtr loadModel();
diff --git a/tests/stage_mockups.cpp b/tests/stage_mockups.cpp
new file mode 100644
index 0000000..78f0656
--- /dev/null
+++ b/tests/stage_mockups.cpp
@@ -0,0 +1,163 @@
+#include <moveit/task_constructor/stage_p.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+#include "stage_mockups.h"
+
+namespace moveit {
+namespace task_constructor {
+
+unsigned int GeneratorMockup::id_ = 0;
+unsigned int MonitoringGeneratorMockup::id_ = 0;
+unsigned int ConnectMockup::id_ = 0;
+unsigned int PropagatorMockup::id_ = 0;
+unsigned int ForwardMockup::id_ = 0;
+unsigned int BackwardMockup::id_ = 0;
+
+void resetMockupIds()
+{
+  GeneratorMockup::id_ = 0;
+  MonitoringGeneratorMockup::id_ = 0;
+  ConnectMockup::id_ = 0;
+  PropagatorMockup::id_ = 0;
+  ForwardMockup::id_ = 0;
+  BackwardMockup::id_ = 0;
+}
+
+PredefinedCosts::PredefinedCosts(std::list<double>&& costs, bool finite)
+  : costs_{ std::move(costs) }, finite_{ finite }
+{}
+
+bool PredefinedCosts::exhausted() const
+{
+  return costs_.empty();
+}
+
+double PredefinedCosts::cost() const
+{
+  // keep the tests going, but this should not happen
+  EXPECT_GT(costs_.size(), 0u);
+  if(costs_.empty())
+    return 0.0;
+
+  double c{ costs_.front() };
+
+  if(finite_ || costs_.size() > 1)
+  {
+    costs_.pop_front();
+  }
+
+  return c;
+}
+
+void DelayingWrapper::compute()
+{
+  if(!delay_.empty())
+  {  // if empty, we don't delay
+    if(delay_.front() == 0)
+      delay_.pop_front();  // we can compute() now
+    else
+    {
+      --delay_.front();  // continue delaying
+      return;
+    }
+  }
+  // forward to child, eventually generating multiple solutions at once
+  WrapperBase::compute();
+}
+
+GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+  : Generator{ "GEN" + std::to_string(++id_) }
+  , costs_{ std::move(costs) }
+  , solutions_per_compute_{ solutions_per_compute }
+{}
+
+void GeneratorMockup::init(const moveit::core::RobotModelConstPtr& robot_model)
+{
+  ps_.reset(new planning_scene::PlanningScene(robot_model));
+  Generator::init(robot_model);
+}
+
+bool GeneratorMockup::canCompute() const
+{
+  // check if runs are being used and if there are still runs left (costs are then never exhausted) or if costs
+  // are being used and they are not exhausted yet
+  return !costs_.exhausted();
+}
+
+void GeneratorMockup::compute()
+{
+  ++runs_;
+
+  for(std::size_t i = 0; canCompute() && i < solutions_per_compute_; ++i)
+    spawn(InterfaceState(ps_), costs_.cost());
+}
+
+MonitoringGeneratorMockup::MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs)
+  : MonitoringGenerator{ "MON" + std::to_string(++id_), monitored }, costs_{ std::move(costs) }
+{}
+
+void MonitoringGeneratorMockup::onNewSolution(const SolutionBase& s)
+{
+  ++runs_;
+
+  spawn(InterfaceState{ s.end()->scene()->diff() }, SubTrajectory{});
+}
+
+ConnectMockup::ConnectMockup(PredefinedCosts&& costs)
+  : Connecting{ "CON" + std::to_string(++id_) }, costs_{ std::move(costs) }
+{}
+
+void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to)
+{
+  ++runs_;
+
+  auto solution{ std::make_shared<SubTrajectory>() };
+  solution->setCost(costs_.cost());
+  solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost()));
+  connect(from, to, solution);
+}
+
+PropagatorMockup::PropagatorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+  : PropagatingEitherWay{ "PRO" + std::to_string(++id_) }
+  , costs_{ std::move(costs) }
+  , solutions_per_compute_{ solutions_per_compute }
+{}
+
+void PropagatorMockup::computeForward(const InterfaceState& from)
+{
+  ++runs_;
+
+  for(std::size_t i = 0; i < solutions_per_compute_; ++i)
+  {
+    SubTrajectory solution{ robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost() };
+    sendForward(from, InterfaceState{ from.scene()->diff() }, std::move(solution));
+  }
+}
+
+void PropagatorMockup::computeBackward(const InterfaceState& to)
+{
+  ++runs_;
+
+  for(std::size_t i = 0; i < solutions_per_compute_; ++i)
+  {
+    SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
+    sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
+  }
+}
+
+ForwardMockup::ForwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+  : PropagatorMockup{ std::move(costs), solutions_per_compute }
+{
+  restrictDirection(FORWARD);
+  setName("FWD" + std::to_string(++id_));
+}
+
+BackwardMockup::BackwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
+  : PropagatorMockup{ std::move(costs), solutions_per_compute }
+{
+  restrictDirection(BACKWARD);
+  setName("BWD" + std::to_string(++id_));
+}
+
+}  // namespace task_constructor
+}  // namespace moveit
diff --git a/tests/stage_mockups.h b/tests/stage_mockups.h
new file mode 100644
index 0000000..e2e9b01
--- /dev/null
+++ b/tests/stage_mockups.h
@@ -0,0 +1,201 @@
+#pragma once
+
+#include <thread>
+
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/cost_terms.h>
+
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <gtest/gtest.h>
+/* #include <gmock/gmock.h> */
+
+namespace moveit {
+namespace task_constructor {
+
+MOVEIT_STRUCT_FORWARD(PredefinedCosts);
+struct PredefinedCosts : CostTerm
+{
+  mutable std::list<double> costs_;  // list of costs to assign
+  mutable bool finite_;
+
+  PredefinedCosts(std::list<double>&& costs, bool finite = true);
+
+  static PredefinedCosts constant(double c)
+  {
+    return PredefinedCosts{ std::list<double>{ c }, false };
+  }
+  static PredefinedCosts single(double c)
+  {
+    return PredefinedCosts{ std::list<double>{ c }, true };
+  }
+
+  bool exhausted() const;
+  double cost() const;
+
+  double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override
+  {
+    return cost();
+  }
+  double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override
+  {
+    return cost();
+  }
+  double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override
+  {
+    return cost();
+  }
+};
+
+constexpr double INF{ std::numeric_limits<double>::infinity() };
+
+/* wrapper stage to delay solutions by a given number of steps */
+struct DelayingWrapper : public WrapperBase
+{
+  std::list<unsigned int> delay_;
+  /* delay list specifies the number of steps each received solution should be delayed */
+  DelayingWrapper(std::list<unsigned int> delay, Stage::pointer&& child)
+    : WrapperBase("delayer", std::move(child)), delay_{ std::move(delay) }
+  {}
+
+  void compute() override;
+  void onNewSolution(const SolutionBase& s) override
+  {
+    liftSolution(s);
+  }
+};
+
+struct GeneratorMockup : public Generator
+{
+  planning_scene::PlanningScenePtr ps_;
+
+  PredefinedCosts costs_;
+  size_t runs_{ 0 };
+  std::size_t solutions_per_compute_;
+
+  static unsigned int id_;
+
+  // default to one solution to avoid infinity loops
+  GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
+                  std::size_t solutions_per_compute = 1);
+  GeneratorMockup(std::initializer_list<double> costs, std::size_t solutions_per_compute = 1)
+    : GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true }, solutions_per_compute }
+  {}
+
+  void init(const moveit::core::RobotModelConstPtr& robot_model) override;
+  bool canCompute() const override;
+  void compute() override;
+  virtual void reset() override
+  {
+    runs_ = 0;
+  };
+};
+
+struct MonitoringGeneratorMockup : public MonitoringGenerator
+{
+  PredefinedCosts costs_;
+  size_t runs_{ 0 };
+
+  static unsigned int id_;
+
+  MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
+  MonitoringGeneratorMockup(Stage* monitored, std::initializer_list<double> costs)
+    : MonitoringGeneratorMockup{ monitored, PredefinedCosts{ std::list<double>{ costs }, true } }
+  {}
+
+  bool canCompute() const override
+  {
+    return false;
+  }
+  void compute() override
+  {}
+  void onNewSolution(const SolutionBase& s) override;
+  virtual void reset() override
+  {
+    runs_ = 0;
+  };
+};
+
+struct ConnectMockup : public Connecting
+{
+  PredefinedCosts costs_;
+  size_t runs_{ 0 };
+
+  static unsigned int id_;
+
+  ConnectMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
+  ConnectMockup(std::initializer_list<double> costs)
+    : ConnectMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+  {}
+
+  using Connecting::compatible;  // make this accessible for testing
+
+  void compute(const InterfaceState& from, const InterfaceState& to) override;
+  virtual void reset() override
+  {
+    runs_ = 0;
+  };
+};
+
+struct PropagatorMockup : public PropagatingEitherWay
+{
+  PredefinedCosts costs_;
+  size_t runs_{ 0 };
+  std::size_t solutions_per_compute_;
+
+  static unsigned int id_;
+
+  PropagatorMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
+  PropagatorMockup(std::initializer_list<double> costs)
+    : PropagatorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+  {}
+
+  void computeForward(const InterfaceState& from) override;
+  void computeBackward(const InterfaceState& to) override;
+  virtual void reset() override
+  {
+    runs_ = 0;
+  };
+};
+
+struct ForwardMockup : public PropagatorMockup
+{
+  static unsigned int id_;
+
+  ForwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
+  ForwardMockup(std::initializer_list<double> costs)
+    : ForwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+  {}
+};
+
+struct BackwardMockup : public PropagatorMockup
+{
+  static unsigned int id_;
+
+  BackwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
+  BackwardMockup(std::initializer_list<double> costs)
+    : BackwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } }
+  {}
+};
+
+// ForwardMockup that takes a while for its computation
+class TimedForwardMockup : public ForwardMockup
+{
+  std::chrono::milliseconds duration_;
+
+public:
+  TimedForwardMockup(std::chrono::milliseconds duration)
+    : ForwardMockup{}, duration_{ duration }
+  {}
+  void computeForward(const InterfaceState& from) override
+  {
+    std::this_thread::sleep_for(duration_);
+    ForwardMockup::computeForward(from);
+  }
+};
+
+// reset ids of all Mockup types (used to generate unique stage names)
+void resetMockupIds();
+
+}  // namespace task_constructor
+}  // namespace moveit

From 9551a487bb016a933c9a07f23879808fedf066b8 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Wed, 6 Nov 2024 14:01:22 -0500
Subject: [PATCH 03/15] Fix double delete (#27)

* [refac] template stage/container moves to container/task

* [fix] container double delete
Remove container OutputPort. Pass directly the task to add stages.

* Update examples
---
 CMakeLists.txt                                |  2 -
 examples/bt_allow_collisions.cpp              | 12 ++--
 examples/bt_compute_ik.cpp                    | 26 ++++----
 examples/bt_create_collision_object.cpp       |  4 --
 examples/bt_demo.cpp                          |  8 +--
 examples/bt_generate_place_pose.cpp           | 64 +++++++++----------
 examples/bt_joint_interpolation_solver.cpp    | 12 ++--
 examples/bt_move_relative.cpp                 | 14 ++--
 examples/bt_move_to.cpp                       | 21 +++---
 examples/bt_serial_container.cpp              | 18 +++---
 .../move_mtc_container_to_parent_container.h  | 18 ------
 include/behaviortree_mtc/move_mtc_stage.h     | 63 ++++++++++++++++++
 .../move_mtc_stage_to_container.h             | 18 ------
 src/initialize_mtc_task.cpp                   |  6 --
 ...move_mtc_container_to_parent_container.cpp | 54 ----------------
 src/move_mtc_stage_to_container.cpp           | 54 ----------------
 16 files changed, 151 insertions(+), 243 deletions(-)
 delete mode 100644 include/behaviortree_mtc/move_mtc_container_to_parent_container.h
 create mode 100644 include/behaviortree_mtc/move_mtc_stage.h
 delete mode 100644 include/behaviortree_mtc/move_mtc_stage_to_container.h
 delete mode 100644 src/move_mtc_container_to_parent_container.cpp
 delete mode 100644 src/move_mtc_stage_to_container.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0c87c4e..36f874f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,8 +52,6 @@ add_library(${PROJECT_NAME}
   src/get_mtc_raw_stage.cpp
   src/initialize_mtc_task.cpp
   src/moveit_msgs.cpp
-  src/move_mtc_stage_to_container.cpp
-  src/move_mtc_container_to_parent_container.cpp
   src/plan_mtc_task.cpp
 )
 
diff --git a/examples/bt_allow_collisions.cpp b/examples/bt_allow_collisions.cpp
index c3211b4..734251e 100644
--- a/examples/bt_allow_collisions.cpp
+++ b/examples/bt_allow_collisions.cpp
@@ -2,7 +2,7 @@
 
 #include <behaviortree_mtc/initialize_mtc_task.h>
 #include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 #include <behaviortree_mtc/moveit_msgs.h>
 #include <behaviortree_mtc/geometry_msgs.h>
@@ -62,23 +62,23 @@ static const char* xml_text = R"(
                                                 collision_object="{cylinder}"  />
             <AddObjectToPlanningScene  planning_scene_interface="{psi}" 
                                        collision_object="{cylinder}"/> 
-            <InitializeMTCTask        task="{mtc_task}" container="{task_container}" />
+            <InitializeMTCTask        task="{mtc_task}" />
             <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}"
                                  goal_joint_tolerance="1e-5" />
             <CreateMTCCurrentState    stage="{current_state}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{current_state}" />
+            <MoveMTCStageToTask  child="{current_state}" parent="{mtc_task}" />
             <AllowCollisionsJointModelGroup stage_name="allow collisions -> hand,object"
                                             stage="{allow_collisions}"
                                             objects="{object_name}"
                                             jmg_name="{hand_group_name}"
                                             task="{mtc_task}" />
-            <MoveMTCStageToContainer  container="{task_container}"  stage="{allow_collisions}" />
+            <MoveMTCStageToTask  child="{allow_collisions}" parent="{mtc_task}" />
             <ForbidAllCollisions  stage_name="forbid collisions -> hand,object"
                                   stage="{forbid_collisions}"
                                   objects="{object_name}" />
-            <MoveMTCStageToContainer  container="{task_container}"  stage="{forbid_collisions}" />
+            <MoveMTCStageToTask  child="{forbid_collisions}" parent="{mtc_task}" />
             <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
         </Sequence>
      </BehaviorTree>
@@ -102,10 +102,10 @@ int main(int argc, char** argv)
   factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
   factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
   factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
   factory.registerNodeType<AllowCollisionPairs>("AllowCollisionPairs");
   factory.registerNodeType<ForbidAllCollisions>("ForbidAllCollisions");
diff --git a/examples/bt_compute_ik.cpp b/examples/bt_compute_ik.cpp
index d2a254a..9b62e9a 100644
--- a/examples/bt_compute_ik.cpp
+++ b/examples/bt_compute_ik.cpp
@@ -2,8 +2,7 @@
 
 #include <behaviortree_mtc/initialize_mtc_task.h>
 #include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/move_mtc_container_to_parent_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/create_mtc_serial_container.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 #include <behaviortree_mtc/moveit_msgs.h>
@@ -69,33 +68,33 @@ static const char* xml_text = R"(
                                                 collision_object="{cylinder}"  />
             <AddObjectToPlanningScene  planning_scene_interface="{psi}" 
                          collision_object="{cylinder}"/> 
-            <InitializeMTCTask        task="{mtc_task}" container="{task_container}" />
+            <InitializeMTCTask        task="{mtc_task}" />
             <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}"
                                  goal_joint_tolerance="1e-5" />
             <CreateMTCCurrentState    stage="{current_state}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{current_state}" />
+            <MoveMTCStageToTask  child="{current_state}" parent="{mtc_task}" />
             <GeometryMsgsPoseStamped  frame_id="{hand_frame}" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
             <CreateMTCMoveToNamedJointPose name="move to -> close_hand motion"
                                       group="{hand_group_name}"
                                       solver="{rrt_connect}"
                                       goal="close"
                                       stage="{stage_move_to_close_hand}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{stage_move_to_close_hand}" />
+            <MoveMTCStageToTask  child="{stage_move_to_close_hand}" parent="{mtc_task}" />
             <CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
                                       group="{hand_group_name}"
                                       solver="{rrt_connect}"
                                       goal="open"
                                       stage="{stage_move_to_open_hand}" />
-            <GetMTCRawStage  stage="{stage_move_to_open_hand}" raw_stage="{raw_stage}"  />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{stage_move_to_open_hand}" />
+            <GetMTCRawStage  stage="{stage_move_to_open_hand}" raw_stage="{raw_stage}" />
+            <MoveMTCStageToTask  child="{stage_move_to_open_hand}" parent="{mtc_task}" />
             <CreateMTCConnect  stage_name="MoveToPick"
                                timeout="5.0"
                                group="{arm_group_name}"
                                planner="{rrt_connect}"
                                stage="{connect}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{connect}" />  
+            <MoveMTCStageToTask  child="{connect}" parent="{mtc_task}" />
             <CreateMTCSerialContainer  container_name="pick object"
                                        container="{pick_object}" />
             <GeometryMsgsVector3Stamped  frame_id="{hand_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
@@ -107,7 +106,7 @@ static const char* xml_text = R"(
                                              direction="{tcp_translate}"
                                              min_distance="0.1"
                                              max_distance="0.15" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{approach_object}" />    
+            <MoveMTCStageToContainer  child="{approach_object}" parent="{pick_object}" />
             <CreateMTCGenerateGraspPose  stage_name="generate_grasp_pose"
                                          eef="{eef}"
                                          object="{object_name}"
@@ -127,8 +126,8 @@ static const char* xml_text = R"(
             <ConfigureInitFromMTCProperties  stage="{grasp_pose_IK}"
                                              property_names="target_pose"
                                              source_flag="INTERFACE" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{grasp_pose_IK}" />
-            <MoveMTCContainerToParentContainer parent_container="{task_container}"  child_container="{pick_object}" />
+            <MoveMTCStageToContainer child="{grasp_pose_IK}" parent="{pick_object}" />
+            <MoveMTCContainerToTask  child="{pick_object}" parent="{mtc_task}" />
             <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
         </Sequence>
      </BehaviorTree>
@@ -154,7 +153,6 @@ int main(int argc, char** argv)
   factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
   factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
   factory.registerNodeType<CreateMTCGenerateGraspPose>("CreateMTCGenerateGraspPose");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
@@ -162,7 +160,9 @@ int main(int argc, char** argv)
   factory.registerNodeType<GetMTCRawStage>("GetMTCRawStage");
   factory.registerNodeType<CreateMTCMoveToNamedJointPose>("CreateMTCMoveToNamedJointPose");
   factory.registerNodeType<CreateMTCConnect>("CreateMTCConnect");
-  factory.registerNodeType<MoveMTCContainerToParentContainer>("MoveMTCContainerToParentContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::ContainerBase>>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
   factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
   factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
   factory.registerNodeType<CreateMTCComputeIK>("CreateMTCComputeIK");
diff --git a/examples/bt_create_collision_object.cpp b/examples/bt_create_collision_object.cpp
index 828475c..5e00099 100644
--- a/examples/bt_create_collision_object.cpp
+++ b/examples/bt_create_collision_object.cpp
@@ -1,9 +1,5 @@
 #include <ros/ros.h>
 
-#include <behaviortree_mtc/initialize_mtc_task.h>
-#include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/plan_mtc_task.h>
 #include <behaviortree_mtc/moveit_msgs.h>
 #include <behaviortree_mtc/geometry_msgs.h>
 #include <behaviortree_mtc/create_planning_scene_interface.h>
diff --git a/examples/bt_demo.cpp b/examples/bt_demo.cpp
index 1c5b913..f60746b 100644
--- a/examples/bt_demo.cpp
+++ b/examples/bt_demo.cpp
@@ -2,7 +2,7 @@
 
 #include <behaviortree_mtc/initialize_mtc_task.h>
 #include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 
 #include "behaviortree_cpp/bt_factory.h"
@@ -19,9 +19,9 @@ static const char* xml_text = R"(
 
      <BehaviorTree ID="MainTree">
         <Sequence name="root">
-            <InitializeMTCTask        task="{mtc_task}" container="{container}" />
+            <InitializeMTCTask        task="{mtc_task}" />
             <CreateMTCCurrentState    stage="{stage}" />
-            <MoveMTCStageToContainer  container="{container}" stage="{stage}" />
+            <MoveMTCStageToTask       child="{stage}" parent="{mtc_task}" />
             <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
         </Sequence>
      </BehaviorTree>
@@ -41,7 +41,7 @@ int main(int argc, char** argv)
 
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
 
   auto tree = factory.createTreeFromText(xml_text);
diff --git a/examples/bt_generate_place_pose.cpp b/examples/bt_generate_place_pose.cpp
index 6f34cbe..567af30 100644
--- a/examples/bt_generate_place_pose.cpp
+++ b/examples/bt_generate_place_pose.cpp
@@ -2,8 +2,7 @@
 
 #include <behaviortree_mtc/initialize_mtc_task.h>
 #include <behaviortree_mtc/create_mtc_current_state.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/move_mtc_container_to_parent_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/create_mtc_serial_container.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 #include <behaviortree_mtc/moveit_msgs.h>
@@ -72,7 +71,7 @@ static const char* xml_text = R"(
                                                 collision_object="{cylinder}"  />
             <AddObjectToPlanningScene  planning_scene_interface="{psi}" 
                          collision_object="{cylinder}"/> 
-            <InitializeMTCTask        task="{mtc_task}" container="{task_container}" />
+            <InitializeMTCTask        task="{mtc_task}" />
             <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}"
@@ -81,7 +80,7 @@ static const char* xml_text = R"(
                                      max_velocity_scaling_factor="1.0"
                                      max_acceleration_scaling_factor="1.0" />
             <CreateMTCCurrentState    stage="{current_state}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{current_state}" />
+            <MoveMTCStageToTask child="{current_state}" parent="{mtc_task}"  />
             <GeometryMsgsPoseStamped  frame_id="{hand_frame}" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
             <CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
                                       group="{hand_group_name}"
@@ -89,13 +88,13 @@ static const char* xml_text = R"(
                                       goal="open"
                                       stage="{stage_move_to_open_hand}" />
             <GetMTCRawStage  stage="{stage_move_to_open_hand}" raw_stage="{raw_stage}"  />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{stage_move_to_open_hand}" />
+            <MoveMTCStageToTask child="{stage_move_to_open_hand}" parent="{mtc_task}"  />
             <CreateMTCConnect  stage_name="MoveToPick"
                                timeout="5.0"
                                group="{arm_group_name}"
                                planner="{rrt_connect}"
                                stage="{connect}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{connect}" />  
+            <MoveMTCStageToTask child="{connect}" parent="{mtc_task}"  />
             <CreateMTCSerialContainer  container_name="pick object"
                                        container="{pick_object}" />
             <GeometryMsgsVector3Stamped  frame_id="{hand_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
@@ -107,7 +106,7 @@ static const char* xml_text = R"(
                                              direction="{tcp_translate}"
                                              min_distance="0.1"
                                              max_distance="0.15" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{approach_object}" />    
+            <MoveMTCStageToContainer child="{approach_object}" parent="{pick_object}" />
             <CreateMTCGenerateGraspPose  stage_name="generate_grasp_pose"
                                          eef="{eef}"
                                          object="{object_name}"
@@ -127,31 +126,31 @@ static const char* xml_text = R"(
             <ConfigureInitFromMTCProperties  stage="{grasp_pose_IK}"
                                              property_names="target_pose"
                                              source_flag="INTERFACE" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{grasp_pose_IK}" />
+            <MoveMTCStageToContainer child="{grasp_pose_IK}" parent="{pick_object}" />
             <AllowCollisionsJointModelGroup stage_name="allow collisions -> hand,object"
                                             stage="{allow_collisions}"
                                             objects="{object_name}"
                                             jmg_name="{hand_group_name}"
                                             task="{mtc_task}" />
-            <MoveMTCStageToContainer  container="{pick_object}"  stage="{allow_collisions}" />
+            <MoveMTCStageToContainer child="{allow_collisions}" parent="{pick_object}" />
             <CreateMTCMoveToNamedJointPose name="close hand"
-                                          group="{hand_group_name}"         
-                                          solver="{rrt_connect}"
+                                           group="{hand_group_name}"
+                                           solver="{rrt_connect}"
                                            goal="close"
-                                            stage="{stage_move_to_close_hand}" />
-             <MoveMTCStageToContainer  container="{pick_object}" stage="{stage_move_to_close_hand}" />
-             <CreateMTCModifyPlanningSceneAttachObjects  stage_name="attach object"
+                                           stage="{stage_move_to_close_hand}" />
+            <MoveMTCStageToContainer child="{stage_move_to_close_hand}" parent="{pick_object}" />
+            <CreateMTCModifyPlanningSceneAttachObjects  stage_name="attach object"
                                                          stage="{attach_object}"
                                                          object_names="{object_name}"
                                                          link_name="{hand_frame}" />
-            <MoveMTCStageToContainer  container="{pick_object}"  stage="{attach_object}" />
+            <MoveMTCStageToContainer child="{attach_object}" parent="{pick_object}" />
             <AllowCollisionPairs  stage_name="allow collisions -> object,support"
                                   stage="{allow_collisions_table}"
                                   first="{object_name}"
                                   second="table" /> 
-            <MoveMTCStageToContainer  container="{pick_object}"  stage="{allow_collisions_table}" />
+            <MoveMTCStageToContainer child="{allow_collisions_table}" parent="{pick_object}" />
             <GeometryMsgsVector3Stamped  frame_id="{reference_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
-          <CreateMTCMoveRelativeTranslate  stage_name="lift object"
+            <CreateMTCMoveRelativeTranslate  stage_name="lift object"
                                            stage="{lift_object}"
                                            group="{arm_group_name}"
                                            solver="{cartesian}"
@@ -159,24 +158,24 @@ static const char* xml_text = R"(
                                            direction="{tcp_translate}"
                                            min_distance="0.01"
                                            max_distance="0.1" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{lift_object}" />
+            <MoveMTCStageToContainer child="{lift_object}" parent="{pick_object}" />
             <ForbidCollisionPairs  stage_name="forbid collisions -> object,support"
                                    stage="{forbid_collisions_table}"
                                    first="{object_name}"
                                    second="table" />
-             <GetMTCRawStage  stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}"  /> 
-             <MoveMTCStageToContainer  container="{pick_object}"  stage="{forbid_collisions_table}" />
-            <MoveMTCContainerToParentContainer parent_container="{task_container}"  child_container="{pick_object}" />
-             <CreateMTCConnect  stage_name="Move To Place"
+            <GetMTCRawStage  stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}"  />
+            <MoveMTCStageToContainer child="{forbid_collisions_table}" parent="{pick_object}" />
+            <MoveMTCContainerToTask child="{pick_object}" parent="{mtc_task}" />
+            <CreateMTCConnect  stage_name="Move To Place"
                                 timeout="5.0"
                                 group="{arm_group_name}"
                                 planner="{rrt_connect}"
                                 stage="{connectPlace}" />
-             <MoveMTCStageToContainer  container="{task_container}" stage="{connectPlace}" />
-             <CreateMTCSerialContainer  container_name="place object"
-                                        container="{place_object}" />
-             <GeometryMsgsVector3Stamped  frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate2}"/>
-             <CreateMTCMoveRelativeTranslate  stage_name="lower object"
+            <MoveMTCStageToTask child="{connectPlace}" parent="{mtc_task}"  />
+            <CreateMTCSerialContainer  container_name="place object"
+                                       container="{place_object}" />
+            <GeometryMsgsVector3Stamped  frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate2}"/>
+            <CreateMTCMoveRelativeTranslate  stage_name="lower object"
                                               stage="{lower_object}"
                                               group="{arm_group_name}"
                                               solver="{cartesian}"
@@ -184,7 +183,7 @@ static const char* xml_text = R"(
                                               direction="{tcp_translate2}"
                                               min_distance="0.03"
                                               max_distance="0.13" />
-            <MoveMTCStageToContainer  container="{place_object}" stage="{lower_object}" />
+            <MoveMTCStageToContainer child="{lower_object}"  parent="{place_object}" />
             <GeometryMsgsPoseStamped  frame_id="world" position="0.6,-0.15,0.1251" quaternion="1,0,0,0" pose_stamped="{place_pose}" />
             <CreateMTCGeneratePlacePose  stage="{generate_place_pose}"
                                          stage_name="generate place pose"
@@ -201,8 +200,8 @@ static const char* xml_text = R"(
             <ConfigureInitFromMTCProperties  stage="{place_pose_IK}"
                                              property_names="target_pose"
                                              source_flag="INTERFACE" />
-            <MoveMTCStageToContainer  container="{place_object}" stage="{place_pose_IK}" />
-            <MoveMTCContainerToParentContainer  parent_container="{task_container}"  child_container="{place_object}" />
+            <MoveMTCStageToContainer child="{place_pose_IK}"  parent="{place_object}" />
+            <MoveMTCContainerToTask child="{place_object}" parent="{mtc_task}" />
             <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
         </Sequence>
      </BehaviorTree>
@@ -228,7 +227,6 @@ int main(int argc, char** argv)
   factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
   factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
   factory.registerNodeType<CreateMTCGenerateGraspPose>("CreateMTCGenerateGraspPose");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
@@ -236,7 +234,9 @@ int main(int argc, char** argv)
   factory.registerNodeType<GetMTCRawStage>("GetMTCRawStage");
   factory.registerNodeType<CreateMTCMoveToNamedJointPose>("CreateMTCMoveToNamedJointPose");
   factory.registerNodeType<CreateMTCConnect>("CreateMTCConnect");
-  factory.registerNodeType<MoveMTCContainerToParentContainer>("MoveMTCContainerToParentContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::ContainerBase>>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
   factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
   factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
   factory.registerNodeType<CreateMTCComputeIK>("CreateMTCComputeIK");
diff --git a/examples/bt_joint_interpolation_solver.cpp b/examples/bt_joint_interpolation_solver.cpp
index 368faa4..a2b38a3 100644
--- a/examples/bt_joint_interpolation_solver.cpp
+++ b/examples/bt_joint_interpolation_solver.cpp
@@ -3,7 +3,7 @@
 #include <behaviortree_mtc/initialize_mtc_task.h>
 #include <behaviortree_mtc/create_mtc_current_state.h>
 #include <behaviortree_mtc/create_mtc_move_relative.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/create_mtc_joint_interpolation.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 
@@ -23,10 +23,10 @@ static const char* xml_text = R"(
 
    <BehaviorTree ID="MainTree">
      <Sequence name="root">
-       <InitializeMTCTask        task="{mtc_task}" container="{container}" />
+       <InitializeMTCTask        task="{mtc_task}" />
        <CreateMTCJointInterpolation  solver="{joint_solver}" />
        <CreateMTCCurrentState        stage="{stage}" />
-       <MoveMTCStageToContainer      container="{container}" stage="{stage}" />
+       <MoveMTCStageToTask      child="{stage}" parent="{mtc_task}" />
        <!-- Joint Motion -> return end joint to default value -->
        <GeometryMsgsPoseStamped  frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
        <CreateMTCMoveRelativeJoint name="move relative -> joint motion"
@@ -35,8 +35,8 @@ static const char* xml_text = R"(
                                    ik_frame="{ik_frame}"
                                    direction="panda_joint7:-1.57079632679"
                                    stage="{stage_move_rel_joint}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_rel_joint}" />
-       <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
+       <MoveMTCStageToTask         child="{stage_move_rel_joint}" parent="{mtc_task}" />
+       <PlanMTCTask                task="{mtc_task}" max_solutions="5" />
      </Sequence>
    </BehaviorTree>
 
@@ -57,7 +57,7 @@ int main(int argc, char** argv)
   factory.registerNodeType<CreateMTCJointInterpolation>("CreateMTCJointInterpolation");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
   factory.registerNodeType<CreateMTCMoveRelativeJoint>("CreateMTCMoveRelativeJoint");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
 
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
diff --git a/examples/bt_move_relative.cpp b/examples/bt_move_relative.cpp
index 4f6d02e..c00e360 100644
--- a/examples/bt_move_relative.cpp
+++ b/examples/bt_move_relative.cpp
@@ -4,7 +4,7 @@
 #include <behaviortree_mtc/create_mtc_current_state.h>
 #include <behaviortree_mtc/create_mtc_move_relative.h>
 #include <behaviortree_mtc/create_mtc_pipeline_planner.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 
 #include <behaviortree_mtc/geometry_msgs.h>
@@ -23,12 +23,12 @@ static const char* xml_text = R"(
 
    <BehaviorTree ID="MainTree">
      <Sequence name="root">
-       <InitializeMTCTask        task="{mtc_task}" container="{container}" />
+       <InitializeMTCTask        task="{mtc_task}" />
        <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}" />
        <CreateMTCCurrentState    stage="{stage}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage}" />
+       <MoveMTCStageToTask  child="{stage}" parent="{mtc_task}" />
 
        <!-- Translate Motion -> 200mm towards x axis wrt. panda_link8 -->
        <GeometryMsgsPoseStamped  frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -39,7 +39,7 @@ static const char* xml_text = R"(
                                        ik_frame="{ik_frame}"
                                        direction="{tcp_translate}"
                                        stage="{stage_move_rel_translate}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_rel_translate}" />
+       <MoveMTCStageToTask  child="{stage_move_rel_translate}" parent="{mtc_task}" />
 
        <!-- Twist Motion -->
        <GeometryMsgsPoseStamped  frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -50,7 +50,7 @@ static const char* xml_text = R"(
                                    ik_frame="{ik_frame}"
                                    direction="{tcp_twist}"
                                    stage="{stage_move_rel_twist}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_rel_twist}" />
+       <MoveMTCStageToTask  child="{stage_move_rel_twist}" parent="{mtc_task}" />
 
        <!-- Joint Motion -> return end joint to default value -->
        <GeometryMsgsPoseStamped  frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -60,7 +60,7 @@ static const char* xml_text = R"(
                                    ik_frame="{ik_frame}"
                                    direction="panda_joint7:-1.57079632679"
                                    stage="{stage_move_rel_joint}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_rel_joint}" />
+       <MoveMTCStageToTask  child="{stage_move_rel_joint}" parent="{mtc_task}" />
 
 
        <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
@@ -86,7 +86,7 @@ int main(int argc, char** argv)
   factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
   factory.registerNodeType<CreateMTCMoveRelativeTwist>("CreateMTCMoveRelativeTwist");
   factory.registerNodeType<CreateMTCMoveRelativeJoint>("CreateMTCMoveRelativeJoint");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
 
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
diff --git a/examples/bt_move_to.cpp b/examples/bt_move_to.cpp
index ca40743..85926d2 100644
--- a/examples/bt_move_to.cpp
+++ b/examples/bt_move_to.cpp
@@ -4,7 +4,7 @@
 #include <behaviortree_mtc/create_mtc_current_state.h>
 #include <behaviortree_mtc/create_mtc_move_to.h>
 #include <behaviortree_mtc/create_mtc_pipeline_planner.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 
 #include <behaviortree_mtc/geometry_msgs.h>
@@ -23,12 +23,12 @@ static const char* xml_text = R"(
 
    <BehaviorTree ID="MainTree">
      <Sequence name="root">
-       <InitializeMTCTask        task="{mtc_task}" container="{container}" />
+       <InitializeMTCTask        task="{mtc_task}" />
        <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}"/>
        <CreateMTCCurrentState    stage="{stage}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage}" />
+       <MoveMTCStageToTask child="{stage}" parent="{mtc_task}" />
 
        <!-- joint named position motion -->
        <CreateMTCMoveToNamedJointPose name="move to -> close_hand motion"
@@ -36,13 +36,14 @@ static const char* xml_text = R"(
                                       solver="{rrt_connect}"
                                       goal="close"
                                       stage="{stage_move_to_close_hand}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_to_close_hand}" />
+       <MoveMTCStageToTask  parent="{mtc_task}" child="{stage_move_to_close_hand}" />
+
        <CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
                                       group="hand"
                                       solver="{rrt_connect}"
                                       goal="open"
                                       stage="{stage_move_to_open_hand}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_to_open_hand}" />
+       <MoveMTCStageToTask  child="{stage_move_to_open_hand}" parent="{mtc_task}" />
        
        <!-- joint motion -->
        <CreateMTCMoveToJoint       name="move to -> joint motion"
@@ -50,7 +51,7 @@ static const char* xml_text = R"(
                                    solver="{rrt_connect}"
                                    goal="panda_joint7:-1.57079632679,panda_joint6:3.14"
                                    stage="{stage_move_to_joint}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_to_joint}" />
+       <MoveMTCStageToTask  parent="{mtc_task}" child="{stage_move_to_joint}" />
        
        <!-- cartesian pose motion -->
        <GeometryMsgsPoseStamped frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -61,9 +62,9 @@ static const char* xml_text = R"(
                                 ik_frame="{ik_frame}"
                                 goal="{goal_pose}"
                                 stage="{stage_move_to_pose}" />
-      <MoveMTCStageToContainer  container="{container}" stage="{stage_move_to_pose}" />
+       <MoveMTCStageToTask      child="{stage_move_to_pose}" parent="{mtc_task}" />
       
-      <!-- cartesian point motion -->
+       <!-- cartesian point motion -->
        <GeometryMsgsPointStamped frame_id="panda_link8" point="0,0.05,0" point_stamped="{goal_point}"/>
        <CreateMTCMoveToPoint     name="move to -> cartesian point motion"
                                  group="panda_arm"
@@ -71,7 +72,7 @@ static const char* xml_text = R"(
                                  ik_frame="{ik_frame}"
                                  goal="{goal_point}"
                                  stage="{stage_move_to_point}" />
-      <MoveMTCStageToContainer  container="{container}" stage="{stage_move_to_point}" />
+       <MoveMTCStageToTask  child="{stage_move_to_point}" parent="{mtc_task}" />
        <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
      </Sequence>
    </BehaviorTree>
@@ -95,7 +96,7 @@ int main(int argc, char** argv)
   factory.registerNodeType<CreateMTCMoveToJoint>("CreateMTCMoveToJoint");
   factory.registerNodeType<CreateMTCMoveToPose>("CreateMTCMoveToPose");
   factory.registerNodeType<CreateMTCMoveToPoint>("CreateMTCMoveToPoint");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
 
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
diff --git a/examples/bt_serial_container.cpp b/examples/bt_serial_container.cpp
index b1c4a42..fba0458 100644
--- a/examples/bt_serial_container.cpp
+++ b/examples/bt_serial_container.cpp
@@ -5,8 +5,7 @@
 #include <behaviortree_mtc/create_mtc_move_relative.h>
 #include <behaviortree_mtc/create_mtc_serial_container.h>
 #include <behaviortree_mtc/create_mtc_pipeline_planner.h>
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/move_mtc_container_to_parent_container.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
 #include <behaviortree_mtc/plan_mtc_task.h>
 
 #include <behaviortree_mtc/geometry_msgs.h>
@@ -25,12 +24,12 @@ static const char* xml_text = R"(
 
    <BehaviorTree ID="MainTree">
      <Sequence name="root">
-       <InitializeMTCTask        task="{mtc_task}" container="{container}" />
+       <InitializeMTCTask        task="{mtc_task}" />
        <CreateMTCPipelinePlanner pipeline_id="ompl"
                                  planner_id="RRTConnect"
                                  solver="{rrt_connect}" />
        <CreateMTCCurrentState    stage="{stage}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage}" />
+       <MoveMTCStageToTask child="{stage}" parent="{mtc_task}" />
 
        <!-- Translate Motion -> 200mm towards x axis wrt. panda_link8 -->
        <GeometryMsgsPoseStamped  frame_id="panda_link8" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}"/>
@@ -41,7 +40,7 @@ static const char* xml_text = R"(
                                        ik_frame="{ik_frame}"
                                        direction="{tcp_translate}"
                                        stage="{stage_move_rel_translate}" />
-       <MoveMTCStageToContainer  container="{container}" stage="{stage_move_rel_translate}" />
+       <MoveMTCStageToTask child="{stage_move_rel_translate}" parent="{mtc_task}" />
        <CreateMTCSerialContainer container_name="approach_object"
                                  container="{serial_container}"/>
        <!-- Translate Motion-->
@@ -52,8 +51,8 @@ static const char* xml_text = R"(
                                        ik_frame="{ik_frame}"
                                        direction="{tcp_translate}"
                                        stage="{stage_move_rel_translate2}" />
-       <MoveMTCStageToContainer  container="{serial_container}" stage="{stage_move_rel_translate2}" />
-       <MoveMTCContainerToParentContainer  parent_container="{container}" child_container="{serial_container}" />
+       <MoveMTCStageToContainer  child="{stage_move_rel_translate2}" parent="{serial_container}" />
+       <MoveMTCContainerToTask   child="{serial_container}" parent="{mtc_task}" />
        <PlanMTCTask              task="{mtc_task}" max_solutions="5" />
      </Sequence>
    </BehaviorTree>
@@ -76,8 +75,9 @@ int main(int argc, char** argv)
   factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
   factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
-  factory.registerNodeType<MoveMTCStageToContainer>("MoveMTCStageToContainer");
-  factory.registerNodeType<MoveMTCContainerToParentContainer>("MoveMTCContainerToParentContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::ContainerBase>>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
   factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
 
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
diff --git a/include/behaviortree_mtc/move_mtc_container_to_parent_container.h b/include/behaviortree_mtc/move_mtc_container_to_parent_container.h
deleted file mode 100644
index cfd909f..0000000
--- a/include/behaviortree_mtc/move_mtc_container_to_parent_container.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <behaviortree_cpp/bt_factory.h>
-
-namespace BT {
-namespace MTC {
-
-class MoveMTCContainerToParentContainer : public BT::SyncActionNode
-{
-public:
-  MoveMTCContainerToParentContainer(const std::string& name, const BT::NodeConfig& config);
-
-  BT::NodeStatus tick() override;
-  static BT::PortsList providedPorts();
-};
-
-}  // namespace MTC
-}  // namespace BT
diff --git a/include/behaviortree_mtc/move_mtc_stage.h b/include/behaviortree_mtc/move_mtc_stage.h
new file mode 100644
index 0000000..6314c74
--- /dev/null
+++ b/include/behaviortree_mtc/move_mtc_stage.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <behaviortree_mtc/shared_to_unique.h>
+
+#include <moveit/task_constructor/task.h>
+
+namespace BT {
+namespace MTC {
+
+/// Move MTC child stage/container to parent container/task
+template <typename C, typename P>
+class MoveMTCStage : public BT::SyncActionNode
+{
+  static_assert(std::is_base_of<moveit::task_constructor::Stage, C>::value, "C must inherit from moveit::task_constructor::Stage");
+  static_assert(std::is_base_of<moveit::task_constructor::Stage, P>::value, "P must inherit from moveit::task_constructor::Stage");
+
+public:
+  MoveMTCStage(const std::string& name, const BT::NodeConfig& config)
+    : SyncActionNode(name, config)
+  {}
+
+  BT::NodeStatus tick() override
+  {
+    // Transform stage from shared to unique
+    moveit::task_constructor::Stage::pointer unique_child{ nullptr };
+    if(auto any_child_ptr = getLockedPortContent("child"))
+    {
+      if(auto* stage_ptr = any_child_ptr->template castPtr<std::shared_ptr<C>>())
+      {
+        auto& stage = *stage_ptr;
+
+        unique_child = sharedToUnique(stage);
+        any_child_ptr.assign(nullptr);  // set blackboard value to nullptr
+      }
+    }
+
+    if(auto any_parent_ptr = getLockedPortContent("parent"); unique_child)
+    {
+      if(auto* parent_ptr = any_parent_ptr->template castPtr<std::shared_ptr<P>>())
+      {
+        auto& parent = *parent_ptr;
+
+        parent->add(std::move(unique_child));
+
+        return NodeStatus::SUCCESS;
+      }
+    }
+
+    return NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::shared_ptr<C>>("child"),
+      BT::BidirectionalPort<std::shared_ptr<P>>("parent"),
+    };
+  }
+};
+
+}  // namespace MTC
+}  // namespace BT
diff --git a/include/behaviortree_mtc/move_mtc_stage_to_container.h b/include/behaviortree_mtc/move_mtc_stage_to_container.h
deleted file mode 100644
index 99f1162..0000000
--- a/include/behaviortree_mtc/move_mtc_stage_to_container.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <behaviortree_cpp/bt_factory.h>
-
-namespace BT {
-namespace MTC {
-
-class MoveMTCStageToContainer : public BT::SyncActionNode
-{
-public:
-  MoveMTCStageToContainer(const std::string& name, const BT::NodeConfig& config);
-
-  BT::NodeStatus tick() override;
-  static BT::PortsList providedPorts();
-};
-
-}  // namespace MTC
-}  // namespace BT
diff --git a/src/initialize_mtc_task.cpp b/src/initialize_mtc_task.cpp
index c56ebe4..d046891 100644
--- a/src/initialize_mtc_task.cpp
+++ b/src/initialize_mtc_task.cpp
@@ -15,11 +15,7 @@ BT::NodeStatus InitializeMTCTask::tick()
   auto task = std::make_shared<moveit::task_constructor::Task>();
   task->loadRobotModel();
 
-  //Convert raw pointer to share pointer
-  auto container = std::shared_ptr<moveit::task_constructor::ContainerBase>(task->stages());
-
   setOutput("task", task);
-  setOutput("container", container);
 
   return NodeStatus::SUCCESS;
 }
@@ -29,8 +25,6 @@ BT::PortsList InitializeMTCTask::providedPorts()
   return {
     BT::OutputPort<moveit::task_constructor::TaskPtr>("task", "{mtc_task}",
                                                       "MoveIt Task Constructor task."),
-    BT::OutputPort<moveit::task_constructor::ContainerBasePtr>("container"),
-
   };
 }
 
diff --git a/src/move_mtc_container_to_parent_container.cpp b/src/move_mtc_container_to_parent_container.cpp
deleted file mode 100644
index 41bfd51..0000000
--- a/src/move_mtc_container_to_parent_container.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include <behaviortree_mtc/move_mtc_container_to_parent_container.h>
-#include <behaviortree_mtc/shared_to_unique.h>
-
-#include <moveit/task_constructor/container.h>
-#include <moveit/task_constructor/task.h>
-
-namespace BT {
-namespace MTC {
-
-MoveMTCContainerToParentContainer::MoveMTCContainerToParentContainer(const std::string& name,
-                                                                     const BT::NodeConfig& config)
-  : SyncActionNode(name, config)
-{}
-
-BT::NodeStatus MoveMTCContainerToParentContainer::tick()
-{
-  // Transform container from shared to unique
-  moveit::task_constructor::ContainerBase::pointer unique_child_container{ nullptr };
-  if(auto any_container_ptr = getLockedPortContent("child_container"))
-  {
-    if(auto* container_ptr = any_container_ptr->castPtr<moveit::task_constructor::ContainerBasePtr>())
-    {
-      auto& child_container = *container_ptr;
-
-      unique_child_container = sharedToUnique(child_container);
-      any_container_ptr.assign(nullptr);  // set blackboard value to nullptr
-    }
-  }
-
-  if(auto any_container_ptr = getLockedPortContent("parent_container"); unique_child_container)
-  {
-    if(auto* container_ptr = any_container_ptr->castPtr<moveit::task_constructor::ContainerBasePtr>())
-    {
-      auto& parent_container = *container_ptr;
-
-      parent_container->add(std::move(unique_child_container));
-
-      return NodeStatus::SUCCESS;
-    }
-  }
-
-  return NodeStatus::SUCCESS;
-}
-
-BT::PortsList MoveMTCContainerToParentContainer::providedPorts()
-{
-  return {
-    BT::InputPort<moveit::task_constructor::ContainerBasePtr>("child_container"),
-    BT::BidirectionalPort<moveit::task_constructor::ContainerBasePtr>("parent_container"),
-  };
-}
-
-}  // namespace MTC
-}  // namespace BT
diff --git a/src/move_mtc_stage_to_container.cpp b/src/move_mtc_stage_to_container.cpp
deleted file mode 100644
index db082d5..0000000
--- a/src/move_mtc_stage_to_container.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include <behaviortree_mtc/move_mtc_stage_to_container.h>
-#include <behaviortree_mtc/shared_to_unique.h>
-
-#include <moveit/task_constructor/container.h>
-#include <moveit/task_constructor/task.h>
-
-namespace BT {
-namespace MTC {
-
-MoveMTCStageToContainer::MoveMTCStageToContainer(const std::string& name,
-                                                 const BT::NodeConfig& config)
-  : SyncActionNode(name, config)
-{}
-
-BT::NodeStatus MoveMTCStageToContainer::tick()
-{
-  // Transform stage from shared to unique
-  moveit::task_constructor::Stage::pointer unique_stage{ nullptr };
-  if(auto any_stage_ptr = getLockedPortContent("stage"))
-  {
-    if(auto* stage_ptr = any_stage_ptr->castPtr<moveit::task_constructor::StagePtr>())
-    {
-      auto& stage = *stage_ptr;
-
-      unique_stage = sharedToUnique(stage);
-      any_stage_ptr.assign(nullptr);  // set blackboard value to nullptr
-    }
-  }
-
-  if(auto any_container_ptr = getLockedPortContent("container"); unique_stage)
-  {
-    if(auto* container_ptr = any_container_ptr->castPtr<moveit::task_constructor::ContainerBasePtr>())
-    {
-      auto& container = *container_ptr;
-
-      container->add(std::move(unique_stage));
-
-      return NodeStatus::SUCCESS;
-    }
-  }
-
-  return NodeStatus::FAILURE;
-}
-
-BT::PortsList MoveMTCStageToContainer::providedPorts()
-{
-  return {
-    BT::InputPort<moveit::task_constructor::StagePtr>("stage"),
-    BT::BidirectionalPort<moveit::task_constructor::ContainerBasePtr>("container"),
-  };
-}
-
-}  // namespace MTC
-}  // namespace BT

From 3b8a5bba84f9282857775f7dba77bbaa16cc7f28 Mon Sep 17 00:00:00 2001
From: berg0401 <72279192+berg0401@users.noreply.github.com>
Date: Wed, 6 Nov 2024 16:24:57 -0500
Subject: [PATCH 04/15] Generate MTC XML model  (#22)

---------

Co-authored-by: bob <berg0401@usherbrooke.ca>
Co-authored-by: Captain Yoshi <captain.yoshisaur@gmail.com>
---
 CMakeLists.txt             |   4 +
 config/bt_mtc_model.xml    | 269 +++++++++++++++++++++++++++++++++++++
 src/generate_xml_model.cpp |  94 +++++++++++++
 3 files changed, 367 insertions(+)
 create mode 100644 config/bt_mtc_model.xml
 create mode 100644 src/generate_xml_model.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 36f874f..3a55c60 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -64,6 +64,7 @@ add_executable(bt_joint_interpolation_solver examples/bt_joint_interpolation_sol
 add_executable(bt_move_relative examples/bt_move_relative.cpp)
 add_executable(bt_move_to examples/bt_move_to.cpp)
 add_executable(bt_serial_container examples/bt_serial_container.cpp)
+add_executable(generate_xml_model src/generate_xml_model.cpp)
 
 target_include_directories(bt_allow_collisions PRIVATE ${catkin_INCLUDE_DIRS} include)
 target_link_libraries(bt_allow_collisions ${PROJECT_NAME} ${catkin_LIBRARIES})
@@ -92,6 +93,9 @@ target_link_libraries(bt_move_to ${PROJECT_NAME} ${catkin_LIBRARIES})
 target_include_directories(bt_serial_container PRIVATE ${catkin_INCLUDE_DIRS} include)
 target_link_libraries(bt_serial_container ${PROJECT_NAME} ${catkin_LIBRARIES})
 
+target_include_directories(generate_xml_model PRIVATE ${catkin_INCLUDE_DIRS} include)
+target_link_libraries(generate_xml_model ${PROJECT_NAME} ${catkin_LIBRARIES})
+
 
 add_subdirectory(tests)
 
diff --git a/config/bt_mtc_model.xml b/config/bt_mtc_model.xml
new file mode 100644
index 0000000..c6a8e09
--- /dev/null
+++ b/config/bt_mtc_model.xml
@@ -0,0 +1,269 @@
+<root BTCPP_format="4">
+    <TreeNodesModel>
+        <Action ID="AddObjectToPlanningScene">
+            <input_port name="collision_object" type="std::shared_ptr&lt;moveit_msgs::CollisionObject_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="planning_scene_interface" type="std::shared_ptr&lt;moveit::planning_interface::PlanningSceneInterface&gt;"/>
+        </Action>
+        <Action ID="AllowAllCollisions">
+            <input_port name="objects" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">allow/forbid all collisions for given object/s</input_port>
+            <input_port name="stage_name" type="std::string"/>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+        </Action>
+        <Action ID="AllowCollisionPairs">
+            <input_port name="second" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">second object of collision pair</input_port>
+            <input_port name="first" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">first object of collision pair</input_port>
+            <input_port name="stage_name" type="std::string"/>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+        </Action>
+        <Action ID="AllowCollisionsJointModelGroup">
+            <input_port name="jmg_name" type="std::string">joint model group name</input_port>
+            <input_port name="objects" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">allow/forbid collisions with joint model group links</input_port>
+            <input_port name="stage_name" type="std::string"/>
+            <input_port name="task" type="std::shared_ptr&lt;moveit::task_constructor::Task&gt;">MTC task</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+        </Action>
+        <Action ID="ConfigureInitFromMTCProperties">
+            <inout_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;"/>
+            <input_port name="property_names" type="std::shared_ptr&lt;std::set&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::less&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;"/>
+            <input_port name="source_flag" type="moveit::task_constructor::Stage::PropertyInitializerSource">Name for property initialization sources. {DEFAULT, MANUAL, PARENT, INTERFACE}</input_port>
+        </Action>
+        <Action ID="CreateMTCCartesianPath">
+            <input_port name="step_size" type="double" default="0.010000">step size between consecutive waypoints</input_port>
+            <output_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;" default="{solver}">Planner interface using pipeline motion solver</output_port>
+            <input_port name="precision" type="moveit::core::CartesianPrecision" default="">linear and rotational precision</input_port>
+            <input_port name="min_fraction" type="double" default="1.000000">fraction of motion required for success</input_port>
+            <input_port name="max_acceleration_scaling_factor" type="double" default="0.100000">scale down max acceleration by this factor</input_port>
+            <input_port name="max_velocity_scaling_factor" type="double" default="0.100000">scale down max velocity by this factor</input_port>
+        </Action>
+        <Action ID="CreateMTCComputeIK">
+            <input_port name="ik_frame" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="min_solution_distance" type="double" default="0.100000">minimum distance between seperate IK solutions for the same target</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;"/>
+            <input_port name="wrapped_stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;"/>
+            <input_port name="max_ik_solutions" type="unsigned int" default="1">max ik solutions</input_port>
+            <input_port name="ignore_collisions" type="bool" default="false">ignore collisions, true or false</input_port>
+            <input_port name="group" type="std::string">name of active group (derived from eef if not provided)</input_port>
+            <input_port name="eef" type="std::string">name of end-effector</input_port>
+            <input_port name="stage_name" type="std::string"/>
+        </Action>
+        <Action ID="CreateMTCConnect">
+            <input_port name="merge_mode" type="unsigned char" default="1">0 = SEQUENTIAL, 1 = WAYPOINTS</input_port>
+            <input_port name="group" type="std::string"/>
+            <input_port name="stage_name" type="std::string">connect</input_port>
+            <input_port name="planner" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;"/>
+            <input_port name="max_distance" type="double" default="0.000100">maximally accepted joint configuration distance between trajectory endpoint and goal state</input_port>
+            <input_port name="timeout" type="double" default="1.000000">timeout</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;" default="{connect_stage}">Connect stage</output_port>
+        </Action>
+        <Action ID="CreateMTCCurrentState">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;" default="{mtc_stage}">MoveIt Task Constructor stage.</output_port>
+        </Action>
+        <Action ID="CreateMTCGenerateGraspPose">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;" default="{generate_grasp_pose}">GenerateGraspPose Stage</output_port>
+            <input_port name="monitored_stage" type="moveit::task_constructor::Stage*"/>
+            <input_port name="stage_name" type="std::string"/>
+            <input_port name="pregrasp_pose" type="std::string">pregrasp posture</input_port>
+            <input_port name="object" type="std::string">object on which we generate the grasp poses</input_port>
+            <input_port name="angle_delta" type="double" default="0.100000">angular steps (rad)</input_port>
+            <input_port name="rotation_axis" type="Vector3D" default="0,0,1">rotate object pose about given axis</input_port>
+            <input_port name="eef" type="std::string">name of end-effector</input_port>
+        </Action>
+        <Action ID="CreateMTCGeneratePlacePose">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;" default="{generate_grasp_pose}">GenerateGraspPose Stage</output_port>
+            <input_port name="pose" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">target pose to pass on in spawned states</input_port>
+            <input_port name="allow_z_flip" type="bool" default="false">allow placing objects upside down</input_port>
+            <input_port name="monitored_stage" type="moveit::task_constructor::Stage*"/>
+            <input_port name="stage_name" type="std::string"/>
+            <input_port name="object" type="std::string">object on which we generate the place poses</input_port>
+        </Action>
+        <Action ID="CreateMTCJointInterpolation">
+            <output_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;" default="{solver}">plan using simple interpolation in joint-space</output_port>
+            <input_port name="max_step" type="double" default="0.100000">max joint step</input_port>
+            <input_port name="max_acceleration_scaling_factor" type="double" default="0.100000">scale down max acceleration by this factor</input_port>
+            <input_port name="max_velocity_scaling_factor" type="double" default="0.100000">scale down max velocity by this factor</input_port>
+        </Action>
+        <Action ID="CreateMTCModifyPlanningSceneAttachObjects">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+            <input_port name="object_names" type="std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt;">list of object names/id's.</input_port>
+            <input_port name="link_name" type="std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt;">attach or detach a list of objects to the given link</input_port>
+            <input_port name="stage_name" type="std::string"/>
+        </Action>
+        <Action ID="CreateMTCModifyPlanningSceneDetachObjects">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+            <input_port name="object_names" type="std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt;">list of object names/id's.</input_port>
+            <input_port name="link_name" type="std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt;">attach or detach a list of objects to the given link</input_port>
+            <input_port name="stage_name" type="std::string"/>
+        </Action>
+        <Action ID="CreateMTCMoveRelativeJoint">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MoveRelative stage</output_port>
+            <input_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;">planner interface</input_port>
+            <input_port name="direction" type="std::shared_ptr&lt;std::map&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, double, std::less&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt;, std::allocator&lt;std::pair&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; const, double&gt; &gt; &gt; &gt;">move specified joint variables by given amount</input_port>
+            <input_port name="max_distance" type="double" default="0.000000">maximum distance to move</input_port>
+            <input_port name="ik_frame" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">frame to be moved in Cartesian direction</input_port>
+            <input_port name="min_distance" type="double" default="-1.000000">minimum distance to move</input_port>
+            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="group" type="std::string">name of planning group</input_port>
+            <input_port name="stage_name" type="std::string" default="move relative">stage name</input_port>
+        </Action>
+        <Action ID="CreateMTCMoveRelativeTranslate">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MoveRelative stage</output_port>
+            <input_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;">planner interface</input_port>
+            <input_port name="direction" type="std::shared_ptr&lt;geometry_msgs::Vector3Stamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">translate link along given direction</input_port>
+            <input_port name="max_distance" type="double" default="0.000000">maximum distance to move</input_port>
+            <input_port name="ik_frame" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">frame to be moved in Cartesian direction</input_port>
+            <input_port name="min_distance" type="double" default="-1.000000">minimum distance to move</input_port>
+            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="group" type="std::string">name of planning group</input_port>
+            <input_port name="stage_name" type="std::string" default="move relative">stage name</input_port>
+        </Action>
+        <Action ID="CreateMTCMoveRelativeTwist">
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MoveRelative stage</output_port>
+            <input_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;">planner interface</input_port>
+            <input_port name="direction" type="std::shared_ptr&lt;geometry_msgs::TwistStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">perform twist motion on specified link</input_port>
+            <input_port name="max_distance" type="double" default="0.000000">maximum distance to move</input_port>
+            <input_port name="ik_frame" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">frame to be moved in Cartesian direction</input_port>
+            <input_port name="min_distance" type="double" default="-1.000000">minimum distance to move</input_port>
+            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="group" type="std::string">name of planning group</input_port>
+            <input_port name="stage_name" type="std::string" default="move relative">stage name</input_port>
+        </Action>
+        <Action ID="CreateMTCMoveToJoint">
+            <input_port name="goal" type="std::shared_ptr&lt;std::map&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, double, std::less&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt;, std::allocator&lt;std::pair&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; const, double&gt; &gt; &gt; &gt;">move joints by name to their mapped target value</input_port>
+            <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+            <input_port name="group" type="std::string">name of planning group</input_port>
+            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;">planner interface</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MoveTo stage</output_port>
+        </Action>
+        <Action ID="CreateMTCMoveToNamedJointPose">
+            <input_port name="goal" type="std::string">move joint model group to given named pose</input_port>
+            <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+            <input_port name="group" type="std::string">name of planning group</input_port>
+            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;">planner interface</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MoveTo stage</output_port>
+        </Action>
+        <Action ID="CreateMTCMoveToPoint">
+            <input_port name="goal" type="std::shared_ptr&lt;geometry_msgs::PointStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">move link to given point, keeping current orientation</input_port>
+            <input_port name="ik_frame" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">frame to be moved in Cartesian direction</input_port>
+            <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+            <input_port name="group" type="std::string">name of planning group</input_port>
+            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;">planner interface</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MoveTo stage</output_port>
+        </Action>
+        <Action ID="CreateMTCMoveToPose">
+            <input_port name="goal" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">move link to a given pose</input_port>
+            <input_port name="ik_frame" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;">frame to be moved in Cartesian direction</input_port>
+            <input_port name="stage_name" type="std::string" default="move to">stage name</input_port>
+            <input_port name="group" type="std::string">name of planning group</input_port>
+            <input_port name="timeout" type="double" default="1.000000"/>
+            <input_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;">planner interface</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MoveTo stage</output_port>
+        </Action>
+        <Action ID="CreateMTCPipelinePlanner">
+            <input_port name="max_acceleration_scaling_factor" type="double" default="0.100000">scale down max acceleration by this factor</input_port>
+            <input_port name="num_planning_attempts" type="unsigned int" default="1u">number of planning attempts</input_port>
+            <input_port name="pipeline_id" type="std::string"/>
+            <input_port name="max_velocity_scaling_factor" type="double" default="0.100000">scale down max velocity by this factor</input_port>
+            <input_port name="planner_id" type="std::string"/>
+            <input_port name="display_motion_plans" type="bool" default="false">publish generated solutions on topic display_planned_path</input_port>
+            <input_port name="goal_orientation_tolerance" type="double" default="1e-4">tolerance for reaching orientation goals</input_port>
+            <input_port name="goal_position_tolerance" type="double" default="1e-4">tolerance for reaching position goals</input_port>
+            <input_port name="goal_joint_tolerance" type="double" default="1e-4">tolerance for reaching joint goals</input_port>
+            <input_port name="publish_planning_requests" type="bool" default="false">publish motion planning requests on topic motion_plan_request</input_port>
+            <output_port name="solver" type="std::shared_ptr&lt;moveit::task_constructor::solvers::PlannerInterface&gt;" default="{solver}">Planner interface using pipeline motion solver</output_port>
+        </Action>
+        <Action ID="CreateMTCSerialContainer">
+            <output_port name="container" type="std::shared_ptr&lt;moveit::task_constructor::ContainerBase&gt;">serial container</output_port>
+            <input_port name="container_name" type="std::string">serial container name</input_port>
+        </Action>
+        <Action ID="CreatePlanningSceneInterface">
+            <output_port name="planning_scene_interface" type="std::shared_ptr&lt;moveit::planning_interface::PlanningSceneInterface&gt;">{planning_scene_interface}</output_port>
+        </Action>
+        <Action ID="ForbidAllCollisions">
+            <input_port name="objects" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">allow/forbid all collisions for given object/s</input_port>
+            <input_port name="stage_name" type="std::string"/>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+        </Action>
+        <Action ID="ForbidCollisionPairs">
+            <input_port name="second" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">second object of collision pair</input_port>
+            <input_port name="first" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">first object of collision pair</input_port>
+            <input_port name="stage_name" type="std::string"/>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+        </Action>
+        <Action ID="ForbidCollisionsJointModelGroup">
+            <input_port name="jmg_name" type="std::string">joint model group name</input_port>
+            <input_port name="objects" type="std::shared_ptr&lt;std::vector&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt;, std::allocator&lt;std::__cxx11::basic_string&lt;char, std::char_traits&lt;char&gt;, std::allocator&lt;char&gt; &gt; &gt; &gt; &gt;">allow/forbid collisions with joint model group links</input_port>
+            <input_port name="stage_name" type="std::string"/>
+            <input_port name="task" type="std::shared_ptr&lt;moveit::task_constructor::Task&gt;">MTC task</input_port>
+            <output_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">ModifyPlanningScene stage</output_port>
+        </Action>
+        <Action ID="GeometryMsgsPose">
+            <output_port name="pose" type="std::shared_ptr&lt;geometry_msgs::Pose_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="quaternion" type="Vector4D"/>
+            <input_port name="position" type="Vector3D"/>
+        </Action>
+        <Action ID="GeometryMsgsPoseStamped">
+            <input_port name="quaternion" type="Vector4D"/>
+            <input_port name="position" type="Vector3D"/>
+            <output_port name="pose_stamped" type="std::shared_ptr&lt;geometry_msgs::PoseStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="frame_id" type="std::string"/>
+        </Action>
+        <Action ID="GeometryMsgsTwistStamped">
+            <output_port name="twist_stamped" type="std::shared_ptr&lt;geometry_msgs::TwistStamped_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="angular_velocity" type="Vector3D"/>
+            <input_port name="linear_velocity" type="Vector3D"/>
+            <input_port name="frame_id" type="std::string"/>
+        </Action>
+        <Action ID="GeometryMsgsVector3Stamped">
+            <output_port name="vector3_stamped" type="std::shared_ptr&lt;geometry_msgs::Vector3Stamped_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="vector" type="Vector3D"/>
+            <input_port name="frame_id" type="std::string"/>
+        </Action>
+        <Action ID="GetMTCRawStage">
+            <output_port name="raw_stage" type="moveit::task_constructor::Stage*">MTC stage raw pointer</output_port>
+            <input_port name="stage" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;">MTC stage</input_port>
+        </Action>
+        <Action ID="InitializeMTCTask">
+            <output_port name="task" type="std::shared_ptr&lt;moveit::task_constructor::Task&gt;" default="{mtc_task}">MoveIt Task Constructor task.</output_port>
+        </Action>
+        <Action ID="MoveItMsgsCollisionObjectBox">
+            <input_port name="height" type="double" default="1.0">Box's size along Z axis (m)</input_port>
+            <input_port name="length" type="double" default="1.0">Box's size along X axis (m)</input_port>
+            <input_port name="object_id" type="std::string"/>
+            <input_port name="frame_id" type="std::string"/>
+            <output_port name="collision_object" type="std::shared_ptr&lt;moveit_msgs::CollisionObject_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="width" type="double" default="1.0">Box's size along Y axis (m)</input_port>
+            <input_port name="pose" type="std::shared_ptr&lt;geometry_msgs::Pose_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+        </Action>
+        <Action ID="MoveItMsgsCollisionObjectCylinder">
+            <input_port name="height" type="double" default="1.0">cylinder height (m)</input_port>
+            <input_port name="object_id" type="std::string"/>
+            <input_port name="frame_id" type="std::string"/>
+            <output_port name="collision_object" type="std::shared_ptr&lt;moveit_msgs::CollisionObject_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+            <input_port name="radius" type="double" default="1.0">cylinder radius (m)</input_port>
+            <input_port name="pose" type="std::shared_ptr&lt;geometry_msgs::Pose_&lt;std::allocator&lt;void&gt; &gt; &gt;"/>
+        </Action>
+        <Action ID="MoveMTCContainerToContainer">
+            <inout_port name="parent" type="std::shared_ptr&lt;moveit::task_constructor::ContainerBase&gt;"/>
+            <input_port name="child" type="std::shared_ptr&lt;moveit::task_constructor::ContainerBase&gt;"/>
+        </Action>
+        <Action ID="MoveMTCContainerToTask">
+            <inout_port name="parent" type="std::shared_ptr&lt;moveit::task_constructor::Task&gt;"/>
+            <input_port name="child" type="std::shared_ptr&lt;moveit::task_constructor::ContainerBase&gt;"/>
+        </Action>
+        <Action ID="MoveMTCStageToContainer">
+            <inout_port name="parent" type="std::shared_ptr&lt;moveit::task_constructor::ContainerBase&gt;"/>
+            <input_port name="child" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;"/>
+        </Action>
+        <Action ID="MoveMTCStageToTask">
+            <inout_port name="parent" type="std::shared_ptr&lt;moveit::task_constructor::Task&gt;"/>
+            <input_port name="child" type="std::shared_ptr&lt;moveit::task_constructor::Stage&gt;"/>
+        </Action>
+        <Action ID="PlanMTCTask">
+            <inout_port name="task" type="std::shared_ptr&lt;moveit::task_constructor::Task&gt;"/>
+            <input_port name="max_solutions" type="unsigned long"/>
+        </Action>
+    </TreeNodesModel>
+</root>
diff --git a/src/generate_xml_model.cpp b/src/generate_xml_model.cpp
new file mode 100644
index 0000000..d761b3d
--- /dev/null
+++ b/src/generate_xml_model.cpp
@@ -0,0 +1,94 @@
+#include <behaviortree_mtc/initialize_mtc_task.h>
+#include <behaviortree_mtc/create_mtc_current_state.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
+#include <behaviortree_mtc/create_mtc_serial_container.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include <behaviortree_mtc/moveit_msgs.h>
+#include <behaviortree_mtc/geometry_msgs.h>
+#include <behaviortree_mtc/create_mtc_pipeline_planner.h>
+#include <behaviortree_mtc/create_planning_scene_interface.h>
+#include <behaviortree_mtc/add_object_to_planning_scene.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include <behaviortree_mtc/create_mtc_generate_grasp_pose.h>
+#include <behaviortree_mtc/get_mtc_raw_stage.h>
+#include <behaviortree_mtc/create_mtc_move_to.h>
+#include <behaviortree_mtc/create_mtc_connect.h>
+#include <behaviortree_mtc/create_mtc_move_relative.h>
+#include <behaviortree_mtc/create_mtc_compute_ik.h>
+#include <behaviortree_mtc/configure_init_from_mtc_properties.h>
+#include <behaviortree_mtc/std_containers.h>
+#include <behaviortree_mtc/create_mtc_cartesian_path.h>
+#include <behaviortree_mtc/create_mtc_modify_planning_scene.h>
+#include <behaviortree_mtc/create_mtc_generate_place_pose.h>
+#include <behaviortree_mtc/create_mtc_joint_interpolation.h>
+
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
+#include "behaviortree_cpp/xml_parsing.h"
+
+using namespace BT;
+using namespace BT::MTC;
+
+int main(int argc, char** argv)
+{
+  BehaviorTreeFactory factory;
+
+  factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
+  factory.registerNodeType<GeometryMsgsTwistStamped>("GeometryMsgsTwistStamped");
+  factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
+  factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
+  factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
+  factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
+  factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
+  factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::ContainerBase>>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::ContainerBase>>("MoveMTCContainerToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
+  factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
+  factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
+  factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
+  factory.registerNodeType<CreateMTCCartesianPath>("CreateMTCCartesianPath");
+  factory.registerNodeType<CreateMTCJointInterpolation>("CreateMTCJointInterpolation");
+  factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
+  factory.registerNodeType<GetMTCRawStage>("GetMTCRawStage");
+  factory.registerNodeType<CreateMTCMoveToNamedJointPose>("CreateMTCMoveToNamedJointPose");
+  factory.registerNodeType<CreateMTCMoveToJoint>("CreateMTCMoveToJoint");
+  factory.registerNodeType<CreateMTCMoveToPoint>("CreateMTCMoveToPoint");
+  factory.registerNodeType<CreateMTCMoveToPose>("CreateMTCMoveToPose");
+  factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
+  factory.registerNodeType<CreateMTCMoveRelativeJoint>("CreateMTCMoveRelativeJoint");
+  factory.registerNodeType<CreateMTCMoveRelativeTwist>("CreateMTCMoveRelativeTwist");
+  factory.registerNodeType<CreateMTCConnect>("CreateMTCConnect");
+  factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
+  factory.registerNodeType<CreateMTCComputeIK>("CreateMTCComputeIK");
+  factory.registerNodeType<ConfigureInitFromMTCProperties<moveit::task_constructor::Stage>>("ConfigureInitFromMTCProperties");
+  factory.registerScriptingEnums<moveit::task_constructor::Stage::PropertyInitializerSource>();
+  factory.registerNodeType<CreateMTCModifyPlanningSceneAttachObjects>("CreateMTCModifyPlanningSceneAttachObjects");
+  factory.registerNodeType<CreateMTCModifyPlanningSceneDetachObjects>("CreateMTCModifyPlanningSceneDetachObjects");
+  factory.registerNodeType<ForbidCollisionPairs>("ForbidCollisionPairs");
+  factory.registerNodeType<AllowCollisionPairs>("AllowCollisionPairs");
+  factory.registerNodeType<AllowAllCollisions>("AllowAllCollisions");
+  factory.registerNodeType<ForbidAllCollisions>("ForbidAllCollisions");
+  factory.registerNodeType<AllowCollisionsJointModelGroup>("AllowCollisionsJointModelGroup");
+  factory.registerNodeType<ForbidCollisionsJointModelGroup>("ForbidCollisionsJointModelGroup");
+  factory.registerNodeType<CreateMTCGenerateGraspPose>("CreateMTCGenerateGraspPose");
+  factory.registerNodeType<CreateMTCGeneratePlacePose>("CreateMTCGeneratePlacePose");
+
+  std::string xml_models = BT::writeTreeNodesModelXML(factory);
+  std::string filePath = "./config/bt_mtc_model.xml";
+  std::ofstream file(filePath);
+  if(file.is_open())
+  {
+    file << xml_models;
+    file.close();
+    std::cout << "File saved successfully: " << filePath << std::endl;
+  }
+  else
+  {
+    std::cerr << "Unable to open file: " << filePath << std::endl;
+  }
+
+  return 0;
+}

From 3631b1272caadabcf041e2bd261ea17d09239ab1 Mon Sep 17 00:00:00 2001
From: berg0401 <72279192+berg0401@users.noreply.github.com>
Date: Wed, 6 Nov 2024 21:03:35 -0500
Subject: [PATCH 05/15] Pick and Place demo (#24)

* Pass properties by their parent

* [cleanup] Add comments + update stage/container moving to new API

---------

Co-authored-by: bob <berg0401@usherbrooke.ca>
Co-authored-by: Captain Yoshi <captain.yoshisaur@gmail.com>
---
 CMakeLists.txt                         |   4 +
 examples/bt_pick_place.cpp             | 383 +++++++++++++++++++++++++
 src/create_mtc_compute_ik.cpp          |  11 +-
 src/create_mtc_generate_grasp_pose.cpp |   7 +-
 src/create_mtc_move_relative.cpp       |   6 +-
 src/create_mtc_move_to.cpp             |   5 +-
 6 files changed, 406 insertions(+), 10 deletions(-)
 create mode 100644 examples/bt_pick_place.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3a55c60..50fef4a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,6 +63,7 @@ add_executable(bt_generate_place_pose examples/bt_generate_place_pose.cpp)
 add_executable(bt_joint_interpolation_solver examples/bt_joint_interpolation_solver.cpp)
 add_executable(bt_move_relative examples/bt_move_relative.cpp)
 add_executable(bt_move_to examples/bt_move_to.cpp)
+add_executable(bt_pick_place examples/bt_pick_place.cpp)
 add_executable(bt_serial_container examples/bt_serial_container.cpp)
 add_executable(generate_xml_model src/generate_xml_model.cpp)
 
@@ -87,6 +88,9 @@ target_link_libraries(bt_joint_interpolation_solver ${PROJECT_NAME} ${catkin_LIB
 target_include_directories(bt_move_relative PRIVATE ${catkin_INCLUDE_DIRS} include)
 target_link_libraries(bt_move_relative ${PROJECT_NAME} ${catkin_LIBRARIES})
 
+target_include_directories(bt_pick_place PRIVATE ${catkin_INCLUDE_DIRS} include)
+target_link_libraries(bt_pick_place ${PROJECT_NAME} ${catkin_LIBRARIES})
+
 target_include_directories(bt_move_to PRIVATE ${catkin_INCLUDE_DIRS} include)
 target_link_libraries(bt_move_to ${PROJECT_NAME} ${catkin_LIBRARIES})
 
diff --git a/examples/bt_pick_place.cpp b/examples/bt_pick_place.cpp
new file mode 100644
index 0000000..bb59c8f
--- /dev/null
+++ b/examples/bt_pick_place.cpp
@@ -0,0 +1,383 @@
+#include <ros/ros.h>
+
+#include <behaviortree_mtc/initialize_mtc_task.h>
+#include <behaviortree_mtc/create_mtc_current_state.h>
+#include <behaviortree_mtc/move_mtc_stage.h>
+#include <behaviortree_mtc/create_mtc_serial_container.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include <behaviortree_mtc/moveit_msgs.h>
+#include <behaviortree_mtc/geometry_msgs.h>
+#include <behaviortree_mtc/create_mtc_pipeline_planner.h>
+#include <behaviortree_mtc/create_planning_scene_interface.h>
+#include <behaviortree_mtc/add_object_to_planning_scene.h>
+#include <behaviortree_mtc/plan_mtc_task.h>
+#include <behaviortree_mtc/create_mtc_generate_grasp_pose.h>
+#include <behaviortree_mtc/get_mtc_raw_stage.h>
+#include <behaviortree_mtc/create_mtc_move_to.h>
+#include <behaviortree_mtc/create_mtc_connect.h>
+#include <behaviortree_mtc/create_mtc_move_relative.h>
+#include <behaviortree_mtc/create_mtc_compute_ik.h>
+#include <behaviortree_mtc/configure_init_from_mtc_properties.h>
+#include <behaviortree_mtc/std_containers.h>
+#include <behaviortree_mtc/create_mtc_cartesian_path.h>
+#include <behaviortree_mtc/create_mtc_modify_planning_scene.h>
+#include <behaviortree_mtc/create_mtc_generate_place_pose.h>
+#include <behaviortree_mtc/expose_mtc_properties.h>
+#include <behaviortree_mtc/set_mtc_properties.h>
+
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
+
+using namespace BT;
+using namespace BT::MTC;
+
+// clang-format off
+
+static const char* xml_text = R"(
+
+ <root BTCPP_format="4" >
+
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root">
+
+            <!-- Set Parameters -->
+            <Script code="object_name:='object' " />
+            <Script code="table_name:='table' " />
+            <Script code="reference_frame:='world' " />
+            <Script code="eef:='hand' " />
+            <Script code="angle:=3.141592653589793/12 " />
+            <Script code="arm_group_name:='panda_arm'" />
+            <Script code="hand_group_name:='hand'" />
+            <Script code="hand_frame:='panda_link8'" />
+
+            <!-- PlanningSceneInterface -->
+            <CreatePlanningSceneInterface  planning_scene_interface="{psi}" />
+
+            <!-- Create Table -->
+            <GeometryMsgsPose name="box's pose" position="0.5,-0.25,-0.05" quaternion="1,0,0,0" pose="{box_pose}" />
+            <MoveItMsgsCollisionObjectBox object_id="{table_name}" length="0.4" width="0.5" height="0.1" pose="{box_pose}" frame_id="{reference_frame}" collision_object="{box}" />
+            <AddObjectToPlanningScene planning_scene_interface="{psi}" collision_object="{box}" />
+
+            <!-- Create Cylinder -->
+            <GeometryMsgsPose name="cylinder's pose" position="0.5,-0.25,0.125" quaternion="1,0,0,0" pose="{cylinder_pose}" />
+            <MoveItMsgsCollisionObjectCylinder object_id="{object_name}"
+                                               radius="0.02"
+                                               height="0.25"
+                                               pose="{cylinder_pose}"
+                                               frame_id="{reference_frame}"
+                                               collision_object="{cylinder}" />
+            <AddObjectToPlanningScene planning_scene_interface="{psi}" collision_object="{cylinder}" />
+
+            <!-- Create Task -->
+            <InitializeMTCTask task="{mtc_task}" />
+
+            <!-- Add properpties to the Task -->
+            <SetMTCTaskPropertyString  stage="{mtc_task}"  property_name="eef"  property="{eef}" />
+            <SetMTCTaskPropertyString  stage="{mtc_task}"  property_name="group"  property="{arm_group_name}" />
+            <SetMTCTaskPropertyString  stage="{mtc_task}"  property_name="hand"  property="{hand_group_name}" />
+            <SetMTCTaskPropertyString  stage="{mtc_task}"  property_name="hand_grasping_frame"  property="{hand_frame}" />
+            <SetMTCTaskPropertyString  stage="{mtc_task}"  property_name="ik_frame"  property="{hand_frame}" />
+
+            <!-- Create Solvers -->
+            <CreateMTCPipelinePlanner pipeline_id="ompl" planner_id="RRTConnect" solver="{rrt_connect}" goal_joint_tolerance="1e-5" />
+            <CreateMTCCartesianPath solver="{cartesian}" max_velocity_scaling_factor="1.0" max_acceleration_scaling_factor="1.0" />
+
+            <!-- Current State -->
+            <CreateMTCCurrentState stage="{current_state}" />
+            <MoveMTCStageToTask child="{current_state}" parent="{mtc_task}" />
+
+
+            <!-- Open Hand -->
+            <GeometryMsgsPoseStamped  frame_id="{hand_frame}" position="0,0,0" quaternion="1,0,0,0" pose_stamped="{ik_frame}" />
+            <CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
+                                           group="{hand_group_name}"
+                                           solver="{rrt_connect}"
+                                           goal="open"
+                                           stage="{stage_move_to_open_hand}" />
+            <GetMTCRawStage stage="{stage_move_to_open_hand}" raw_stage="{raw_stage}" />
+            <MoveMTCStageToTask child="{stage_move_to_open_hand}" parent="{mtc_task}" />
+
+
+            <!-- Move to Pick -->
+            <CreateMTCConnect stage_name="MoveToPick"
+                              timeout="5.0"
+                              group="{arm_group_name}"
+                              planner="{rrt_connect}"
+                              stage="{connect}" />
+            <StageConfigureInitFromMTCProperties stage="{connect}" property_names="" source_flag="PARENT" />
+            <MoveMTCStageToTask child="{connect}" parent="{mtc_task}" />
+
+            <!-- Pick Object-->
+            <CreateMTCSerialContainer container_name="pick object" container="{pick_object}" />
+            <ExposeMTCTaskPropertiesToContainer from="{mtc_task}" to="{pick_object}" property_names="group,eef,hand,ik_frame" />
+            <ContainerConfigureInitFromMTCProperties stage="{pick_object}" property_names="group,eef,hand,ik_frame" source_flag="PARENT" />
+
+                <!-- Approach Object -->
+                <GeometryMsgsVector3Stamped frame_id="{hand_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
+                <CreateMTCMoveRelativeTranslate stage_name="approach object"
+                                                stage="{approach_object}"
+                                                solver="{rrt_connect}"
+                                                ik_frame="{ik_frame}"
+                                                direction="{tcp_translate}"
+                                                min_distance="0.1"
+                                                max_distance="0.15" />
+                <StageConfigureInitFromMTCProperties stage="{approach_object}" property_names="group" source_flag="PARENT" />
+                <MoveMTCStageToContainer child="{approach_object}" parent="{pick_object}" />
+
+                <!-- Generate Grasp Pose -->
+                <CreateMTCGenerateGraspPose stage_name="generate_grasp_pose"
+                                            object="{object_name}"
+                                            angle_delta="{angle}"
+                                            pregrasp_pose="open"
+                                            monitored_stage="{raw_stage}"
+                                            stage="{generate_pose}" />
+                <StageConfigureInitFromMTCProperties stage="{generate_pose}" property_names="" source_flag="PARENT" />
+                <GeometryMsgsPoseStamped frame_id="{hand_frame}" position="0,0,0.1" quaternion="0.270595,0.653228,-0.270861,0.653228" pose_stamped="{ik_frame2}"/>
+                <CreateMTCComputeIK stage_name="grasp pose IK"
+                                    wrapped_stage="{generate_pose}"
+                                    max_ik_solutions="8"
+                                    min_solution_distance="1.0"
+                                    stage="{grasp_pose_IK}"
+                                    ik_frame="{ik_frame2}"  />
+                <StageConfigureInitFromMTCProperties  stage="{grasp_pose_IK}" property_names="eef,group" source_flag="PARENT" />
+                <StageConfigureInitFromMTCProperties  stage="{grasp_pose_IK}" property_names="target_pose" source_flag="INTERFACE" />
+                <MoveMTCStageToContainer child="{grasp_pose_IK}" parent="{pick_object}" />
+
+                <!-- Allow Collisions (hand object) -->
+                <AllowCollisionsJointModelGroup stage_name="allow collisions -> hand,object"
+                                                            stage="{allow_collisions_object_robot}"
+                                                            objects="{object_name}"
+                                                            jmg_name="{hand_group_name}"
+                                                            task="{mtc_task}" />
+                <MoveMTCStageToContainer child="{allow_collisions_object_robot}" parent="{pick_object}" />
+
+                <!-- Close Hand -->
+                <CreateMTCMoveToNamedJointPose name="close hand"
+                                               group="{hand_group_name}"
+                                               solver="{rrt_connect}"
+                                               goal="close"
+                                               stage="{stage_move_to_close_hand}" />
+                <MoveMTCStageToContainer child="{stage_move_to_close_hand}" parent="{pick_object}" />
+
+                <!-- Attach Object -->
+                <CreateMTCModifyPlanningSceneAttachObjects stage_name="attach object"
+                                                           stage="{attach_object}"
+                                                           object_names="{object_name}"
+                                                           link_name="{hand_frame}" />
+                <MoveMTCStageToContainer child="{attach_object}" parent="{pick_object}" />
+
+                <!-- Allow Collision (object support) -->
+                <AllowCollisionPairs stage_name="allow collisions -> object,support"
+                                     stage="{allow_collisions_table}"
+                                     first="{object_name}"
+                                     second="table" />
+                <MoveMTCStageToContainer child="{allow_collisions_table}" parent="{pick_object}" />
+
+                <!-- Lift Object -->
+                <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
+                <CreateMTCMoveRelativeTranslate stage_name="lift object"
+                                                stage="{lift_object}"
+                                                solver="{cartesian}"
+                                                ik_frame="{ik_frame}"
+                                                direction="{tcp_translate}"
+                                                min_distance="0.01"
+                                                max_distance="0.1" />
+                <StageConfigureInitFromMTCProperties stage="{lift_object}" property_names="group" source_flag="PARENT" />
+                <MoveMTCStageToContainer child="{lift_object}" parent="{pick_object}" />
+
+                <!-- Forbid Collision (object support) -->
+                <ForbidCollisionPairs  stage_name="forbid collisions -> object,support"
+                                       stage="{forbid_collisions_table}"
+                                       first="{object_name}"
+                                       second="table" />
+                <GetMTCRawStage  stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}"  />
+                <MoveMTCStageToContainer child="{forbid_collisions_table}" parent="{pick_object}" />
+
+                <!-- Add 'pick_object' Container to Task -->
+                <MoveMTCContainerToTask child="{pick_object}" parent="{mtc_task}" />
+
+            <!-- Move to Place -->
+            <CreateMTCConnect stage_name="Move To Place"
+                              timeout="5.0"
+                              group="{arm_group_name}"
+                              planner="{rrt_connect}"
+                              stage="{connectPlace}" />
+            <StageConfigureInitFromMTCProperties stage="{connectPlace}" property_names="" source_flag="PARENT" />
+            <MoveMTCStageToTask child="{connectPlace}" parent="{mtc_task}" />
+
+            <!-- Place Object -->
+            <CreateMTCSerialContainer container_name="place object" container="{place_object}" />
+            <ExposeMTCTaskPropertiesToContainer from="{mtc_task}" to="{place_object}" property_names="group,eef,hand" />
+            <ContainerConfigureInitFromMTCProperties stage="{place_object}" property_names="group,eef,hand" source_flag="PARENT" />
+
+
+                <!-- Lower Object -->
+                <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate2}" />
+                <CreateMTCMoveRelativeTranslate stage_name="lower object"
+                                                stage="{lower_object}"
+                                                solver="{cartesian}"
+                                                ik_frame="{ik_frame}"
+                                                direction="{tcp_translate2}"
+                                                min_distance="0.03"
+                                                max_distance="0.13" />
+                <StageConfigureInitFromMTCProperties stage="{lower_object}" property_names="group" source_flag="PARENT" />
+                <MoveMTCStageToContainer child="{lower_object}" parent="{place_object}" />
+
+                <!-- Generate Place Pose -->
+                <GeometryMsgsPoseStamped frame_id="world" position="0.6,-0.15,0.1251" quaternion="1,0,0,0" pose_stamped="{place_pose}" />
+                <CreateMTCGeneratePlacePose stage="{generate_place_pose}"
+                                            stage_name="generate place pose"
+                                            object="{object_name}"
+                                            pose="{place_pose}"
+                                            monitored_stage="{raw_forbid_collisions_table}" />
+                <CreateMTCComputeIK stage_name="place pose IK"
+                                    wrapped_stage="{generate_place_pose}"
+                                    max_ik_solutions="2"
+                                    stage="{place_pose_IK}"
+                                    ik_frame="{ik_frame2}"  />
+                <StageConfigureInitFromMTCProperties  stage="{place_pose_IK}" property_names="eef,group" source_flag="PARENT" />
+                <StageConfigureInitFromMTCProperties  stage="{place_pose_IK}" property_names="target_pose" source_flag="INTERFACE" />
+                <MoveMTCStageToContainer child="{place_pose_IK}" parent="{place_object}" />
+
+                <!-- Open Hand -->
+                <CreateMTCMoveToNamedJointPose name="move to -> open_hand motion"
+                                          group="{hand_group_name}"
+                                          solver="{rrt_connect}"
+                                          goal="open"
+                                          stage="{stage_move_to_open_hand2}" />
+                <MoveMTCStageToContainer child="{stage_move_to_open_hand2}" parent="{place_object}" />
+
+                <!-- Forbid Collision (hand, object) -->
+                <ForbidCollisionsJointModelGroup stage_name="forbid collisions -> hand,object"
+                                                 stage="{forbid_collisions_object_robot}"
+                                                 objects="{object_name}"
+                                                 jmg_name="{hand_group_name}"
+                                                 task="{mtc_task}" />
+                <MoveMTCStageToContainer child="{forbid_collisions_object_robot}" parent="{place_object}" />
+
+                <!-- Detach Object -->
+                <CreateMTCModifyPlanningSceneDetachObjects stage_name="detach object"
+                                                           stage="{detach_object}"
+                                                           object_names="{object_name}"
+                                                           link_name="{hand_frame}" />
+                <MoveMTCStageToContainer child="{detach_object}" parent="{place_object}" />
+
+                <!-- Retreat Motion -->
+                <GeometryMsgsVector3Stamped frame_id="{hand_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate3}" />
+                <CreateMTCMoveRelativeTranslate stage_name="retreat after place"
+                                                stage="{retreat_after_place}"
+                                                solver="{cartesian}"
+                                                ik_frame="{ik_frame}"
+                                                direction="{tcp_translate3}"
+                                                min_distance="0.12"
+                                                max_distance="0.25" />
+                <StageConfigureInitFromMTCProperties stage="{retreat_after_place}" property_names="group" source_flag="PARENT" />
+                <MoveMTCStageToContainer child="{retreat_after_place}" parent="{place_object}" />
+
+                <!-- Add 'place_object' Container to Task -->
+                <MoveMTCContainerToTask child="{place_object}" parent="{mtc_task}" />
+
+            <!-- Move to Home -->
+            <CreateMTCMoveToNamedJointPose name="move home"
+                                           solver="{rrt_connect}"
+                                           goal="ready"
+                                           stage="{stage_move_to_home}" />
+            <StageConfigureInitFromMTCProperties stage="{stage_move_to_home}" property_names="group" source_flag="PARENT" />
+            <MoveMTCStageToTask child="{stage_move_to_home}" parent="{mtc_task}" />
+
+            <!-- Plan the Task -->
+            <PlanMTCTask task="{mtc_task}" max_solutions="5" />
+        </Sequence>
+     </BehaviorTree>
+
+ </root>
+ )";
+
+// clang-format on
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "test_behavior_tree");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  BehaviorTreeFactory factory;
+
+  factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
+  factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
+  factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
+  factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
+  factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
+  factory.registerNodeType<CreatePlanningSceneInterface>("CreatePlanningSceneInterface");
+  factory.registerNodeType<AddObjectToPlanningScene>("AddObjectToPlanningScene");
+  factory.registerNodeType<CreateMTCGenerateGraspPose>("CreateMTCGenerateGraspPose");
+  factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
+  factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
+  factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
+  factory.registerNodeType<PlanMTCTask>("PlanMTCTask");
+  factory.registerNodeType<GetMTCRawStage>("GetMTCRawStage");
+  factory.registerNodeType<CreateMTCMoveToNamedJointPose>("CreateMTCMoveToNamedJointPose");
+  factory.registerNodeType<CreateMTCConnect>("CreateMTCConnect");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::ContainerBase>>("MoveMTCStageToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::ContainerBase>>("MoveMTCContainerToContainer");
+  factory.registerNodeType<MoveMTCStage<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveMTCContainerToTask");
+  factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
+  factory.registerNodeType<CreateMTCMoveRelativeTranslate>("CreateMTCMoveRelativeTranslate");
+  factory.registerNodeType<CreateMTCComputeIK>("CreateMTCComputeIK");
+  factory.registerNodeType<ExposeMTCProperties<moveit::task_constructor::ContainerBase, moveit::task_constructor::ContainerBase>>("ExposeMTCContainerPropertiesToContainer");
+  factory.registerNodeType<ExposeMTCProperties<moveit::task_constructor::Task, moveit::task_constructor::ContainerBase>>("ExposeMTCTaskPropertiesToContainer");
+  factory.registerNodeType<ConfigureInitFromMTCProperties<moveit::task_constructor::Stage>>("StageConfigureInitFromMTCProperties");
+  factory.registerNodeType<ConfigureInitFromMTCProperties<moveit::task_constructor::ContainerBase>>("ContainerConfigureInitFromMTCProperties");
+  factory.registerNodeType<SetMTCProperties<moveit::task_constructor::Stage, std::string>>("SetMTCStagePropertyString");
+  factory.registerNodeType<SetMTCProperties<moveit::task_constructor::Task, std::string>>("SetMTCTaskPropertyString");
+  factory.registerScriptingEnums<moveit::task_constructor::Stage::PropertyInitializerSource>();
+  factory.registerNodeType<CreateMTCCartesianPath>("CreateMTCCartesianPath");
+  factory.registerNodeType<CreateMTCModifyPlanningSceneAttachObjects>("CreateMTCModifyPlanningSceneAttachObjects");
+  factory.registerNodeType<CreateMTCModifyPlanningSceneDetachObjects>("CreateMTCModifyPlanningSceneDetachObjects");
+  factory.registerNodeType<AllowCollisionPairs>("AllowCollisionPairs");
+  factory.registerNodeType<AllowCollisionsJointModelGroup>("AllowCollisionsJointModelGroup");
+  factory.registerNodeType<ForbidCollisionsJointModelGroup>("ForbidCollisionsJointModelGroup");
+  factory.registerNodeType<ForbidCollisionPairs>("ForbidCollisionPairs");
+  factory.registerNodeType<CreateMTCGeneratePlacePose>("CreateMTCGeneratePlacePose");
+
+  auto tree = factory.createTreeFromText(xml_text);
+
+  // Connect the Groot2Publisher. This will allow Groot2 to
+  // get the tree and poll status updates.
+  const unsigned port = 1667;
+  BT::Groot2Publisher publisher(tree, port);
+
+  // Add two more loggers, to save the transitions into a file.
+  // Both formats are compatible with Groot2
+
+  // Logging with lightweight serialization
+  BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
+
+  // Gives the user time to connect to Groot2
+  int wait_time = 5000;
+  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
+            << std::endl;
+  std::cout << "======================" << std::endl;
+  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+
+  std::cout << "Starting Behavior Tree" << std::endl;
+  std::cout << "======================" << std::endl;
+
+  NodeStatus status = NodeStatus::IDLE;
+  while(ros::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING))
+  {
+    status = tree.tickExactlyOnce();
+    std::cout << "Status = " << status << std::endl;
+
+    ros::Duration sleep_time(0.01);
+    sleep_time.sleep();
+  }
+
+  std::cout << "\n\nWaiting for shutdown, press CTRL-C to quit" << std::endl;
+  ros::waitForShutdown();
+
+  return 0;
+}
diff --git a/src/create_mtc_compute_ik.cpp b/src/create_mtc_compute_ik.cpp
index 683d393..09ea98a 100644
--- a/src/create_mtc_compute_ik.cpp
+++ b/src/create_mtc_compute_ik.cpp
@@ -26,8 +26,6 @@ BT::NodeStatus CreateMTCComputeIK::tick()
   std::shared_ptr<geometry_msgs::PoseStamped> ik_frame{ nullptr };
 
   if(!getInput("stage_name", name) ||
-     !getInput("eef", eef) ||
-     !getInput("group", group) ||
      !getInput("ignore_collisions", ignore_collisions) ||
      !getInput("min_solution_distance", min_solution_distance) ||
      !getInput("max_ik_solutions", max_ik_solutions) ||
@@ -43,8 +41,13 @@ BT::NodeStatus CreateMTCComputeIK::tick()
     new moveit::task_constructor::stages::ComputeIK(name, std::move(unique_stage)),
     dirty::fake_deleter{}
   };
-  wrapper->setEndEffector(eef);
-  wrapper->setGroup(group);
+
+  // Optionnal
+  if(getInput("group", group))
+    wrapper->setGroup(group);
+  if(getInput("eef", eef))
+    wrapper->setEndEffector(eef);  
+
   wrapper->setMaxIKSolutions(max_ik_solutions);
   wrapper->setIgnoreCollisions(ignore_collisions);
   wrapper->setMinSolutionDistance(min_solution_distance);
diff --git a/src/create_mtc_generate_grasp_pose.cpp b/src/create_mtc_generate_grasp_pose.cpp
index ee5bc6f..823e7f1 100644
--- a/src/create_mtc_generate_grasp_pose.cpp
+++ b/src/create_mtc_generate_grasp_pose.cpp
@@ -21,7 +21,6 @@ BT::NodeStatus CreateMTCGenerateGraspPose::tick()
   Eigen::Vector3d rotation_axis;
   moveit::task_constructor::Stage* monitored_stage;
   if(!getInput("stage_name", name) ||
-     !getInput("eef", eef) ||
      !getInput("angle_delta", angle_delta) ||
      !getInput("object", object) ||
      !getInput("pregrasp_pose", pregrasp_pose) ||
@@ -35,10 +34,14 @@ BT::NodeStatus CreateMTCGenerateGraspPose::tick()
     new moveit::task_constructor::stages::GenerateGraspPose(name),
     dirty::fake_deleter{}
   };
+
+  //Optionnal
+  if(getInput("eef", eef))
+    stage->setEndEffector(eef);
+  
   rotation_axis[0] = rotation_axis_input.x;
   rotation_axis[1] = rotation_axis_input.y;
   rotation_axis[2] = rotation_axis_input.z;
-  stage->setEndEffector(eef);
   stage->setObject(object);
   stage->setAngleDelta(angle_delta);
   stage->setRotationAxis(rotation_axis);
diff --git a/src/create_mtc_move_relative.cpp b/src/create_mtc_move_relative.cpp
index d54e425..4401dee 100644
--- a/src/create_mtc_move_relative.cpp
+++ b/src/create_mtc_move_relative.cpp
@@ -23,7 +23,6 @@ BT::NodeStatus CreateMTCMoveRelativeBase::tick(std::shared_ptr<moveit::task_cons
   double timeout;
 
   if(!getInput("stage_name", name) ||
-     !getInput("group", group) ||
      !getInput("solver", solver) ||
      !getInput("ik_frame", ik_frame) ||
      !getInput("min_distance", min_distance) ||
@@ -37,7 +36,10 @@ BT::NodeStatus CreateMTCMoveRelativeBase::tick(std::shared_ptr<moveit::task_cons
     dirty::fake_deleter{}
   };
 
-  stage->setGroup(group);
+  //Optionnal
+  if(getInput("group", group))
+    stage->setGroup(group);
+
   stage->setIKFrame(*ik_frame);
   stage->setMinMaxDistance(min_distance, max_distance);
   stage->setTimeout(timeout);
diff --git a/src/create_mtc_move_to.cpp b/src/create_mtc_move_to.cpp
index fee8241..5992663 100644
--- a/src/create_mtc_move_to.cpp
+++ b/src/create_mtc_move_to.cpp
@@ -22,7 +22,6 @@ BT::NodeStatus CreateMTCMoveToBase::tick(std::shared_ptr<moveit::task_constructo
   double timeout;
 
   if(!getInput("stage_name", name) ||
-     !getInput("group", group) ||
      !getInput("solver", solver) ||
      !getInput("timeout", timeout))
     return NodeStatus::FAILURE;
@@ -32,8 +31,10 @@ BT::NodeStatus CreateMTCMoveToBase::tick(std::shared_ptr<moveit::task_constructo
     new moveit::task_constructor::stages::MoveTo(name, solver),
     dirty::fake_deleter{}
   };
+  // Optionnal
+  if(getInput("group", group))
+    stage->setGroup(group);
 
-  stage->setGroup(group);
   stage->setTimeout(timeout);
 
   return NodeStatus::SUCCESS;

From a83dc6318f6afa1ed8b82ac05fdf8d16c4b44660 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 10:37:57 -0500
Subject: [PATCH 06/15] Fix planning time disparity for pickplace demo (#28)

---
 examples/bt_pick_place.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/examples/bt_pick_place.cpp b/examples/bt_pick_place.cpp
index bb59c8f..54b3bd0 100644
--- a/examples/bt_pick_place.cpp
+++ b/examples/bt_pick_place.cpp
@@ -81,7 +81,7 @@ static const char* xml_text = R"(
 
             <!-- Create Solvers -->
             <CreateMTCPipelinePlanner pipeline_id="ompl" planner_id="RRTConnect" solver="{rrt_connect}" goal_joint_tolerance="1e-5" />
-            <CreateMTCCartesianPath solver="{cartesian}" max_velocity_scaling_factor="1.0" max_acceleration_scaling_factor="1.0" />
+            <CreateMTCCartesianPath solver="{cartesian_planner}" max_velocity_scaling_factor="1.0" max_acceleration_scaling_factor="1.0" />
 
             <!-- Current State -->
             <CreateMTCCurrentState stage="{current_state}" />
@@ -117,7 +117,7 @@ static const char* xml_text = R"(
                 <GeometryMsgsVector3Stamped frame_id="{hand_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
                 <CreateMTCMoveRelativeTranslate stage_name="approach object"
                                                 stage="{approach_object}"
-                                                solver="{rrt_connect}"
+                                                solver="{cartesian_planner}"
                                                 ik_frame="{ik_frame}"
                                                 direction="{tcp_translate}"
                                                 min_distance="0.1"
@@ -178,7 +178,7 @@ static const char* xml_text = R"(
                 <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
                 <CreateMTCMoveRelativeTranslate stage_name="lift object"
                                                 stage="{lift_object}"
-                                                solver="{cartesian}"
+                                                solver="{cartesian_planner}"
                                                 ik_frame="{ik_frame}"
                                                 direction="{tcp_translate}"
                                                 min_distance="0.01"
@@ -216,7 +216,7 @@ static const char* xml_text = R"(
                 <GeometryMsgsVector3Stamped frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate2}" />
                 <CreateMTCMoveRelativeTranslate stage_name="lower object"
                                                 stage="{lower_object}"
-                                                solver="{cartesian}"
+                                                solver="{cartesian_planner}"
                                                 ik_frame="{ik_frame}"
                                                 direction="{tcp_translate2}"
                                                 min_distance="0.03"
@@ -267,7 +267,7 @@ static const char* xml_text = R"(
                 <GeometryMsgsVector3Stamped frame_id="{hand_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate3}" />
                 <CreateMTCMoveRelativeTranslate stage_name="retreat after place"
                                                 stage="{retreat_after_place}"
-                                                solver="{cartesian}"
+                                                solver="{cartesian_planner}"
                                                 ik_frame="{ik_frame}"
                                                 direction="{tcp_translate3}"
                                                 min_distance="0.12"

From 9f7b61f32f03e82c3d59b85f2359bfd57309b556 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 11:01:53 -0500
Subject: [PATCH 07/15] Optionaly delay the start of the examples with a ROS
 Parameter

Gives the user a chance to connect to Groot2.
---
 examples/bt_allow_collisions.cpp           | 17 +++--
 examples/bt_compute_ik.cpp                 | 17 +++--
 examples/bt_create_collision_object.cpp    | 17 +++--
 examples/bt_demo.cpp                       | 17 +++--
 examples/bt_generate_place_pose.cpp        | 84 +++-------------------
 examples/bt_joint_interpolation_solver.cpp | 16 +++--
 examples/bt_move_relative.cpp              | 16 +++--
 examples/bt_move_to.cpp                    | 18 +++--
 examples/bt_pick_place.cpp                 | 17 +++--
 examples/bt_serial_container.cpp           | 16 +++--
 10 files changed, 111 insertions(+), 124 deletions(-)

diff --git a/examples/bt_allow_collisions.cpp b/examples/bt_allow_collisions.cpp
index 734251e..fede627 100644
--- a/examples/bt_allow_collisions.cpp
+++ b/examples/bt_allow_collisions.cpp
@@ -91,12 +91,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
   factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
@@ -125,12 +125,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_compute_ik.cpp b/examples/bt_compute_ik.cpp
index 9b62e9a..4489c8a 100644
--- a/examples/bt_compute_ik.cpp
+++ b/examples/bt_compute_ik.cpp
@@ -139,12 +139,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
   factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
@@ -183,12 +183,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_create_collision_object.cpp b/examples/bt_create_collision_object.cpp
index 5e00099..3dbff87 100644
--- a/examples/bt_create_collision_object.cpp
+++ b/examples/bt_create_collision_object.cpp
@@ -57,12 +57,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("MoveItMsgsCollisionObjectCylinder");
   factory.registerNodeType<MoveItMsgsCollisionObjectBox>("MoveItMsgsCollisionObjectBox");
@@ -83,12 +83,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_demo.cpp b/examples/bt_demo.cpp
index f60746b..5a155b2 100644
--- a/examples/bt_demo.cpp
+++ b/examples/bt_demo.cpp
@@ -33,12 +33,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
   factory.registerNodeType<MoveMTCStage<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveMTCStageToTask");
@@ -58,12 +58,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_generate_place_pose.cpp b/examples/bt_generate_place_pose.cpp
index 567af30..1dcee7f 100644
--- a/examples/bt_generate_place_pose.cpp
+++ b/examples/bt_generate_place_pose.cpp
@@ -213,12 +213,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
   factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
@@ -264,12 +264,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
@@ -288,70 +293,3 @@ int main(int argc, char** argv)
 
   return 0;
 }
-
-/**
- * 
-            <CreateMTCModifyPlanningSceneAttachObjects  stage_name="attach object"
-                                                        stage="{attach_object}"
-                                                        object_names="{object_name}"
-                                                        link_name="{hand_frame}" />
-            <MoveMTCStageToContainer  container="{pick_object}"  stage="{attach_object}" />
-            <AllowCollisionPairs  stage_name="allow collisions -> object,support"
-                                  stage="{allow_collisions_table}"
-                                  first="{object_name}"
-                                  second="table" /> 
-            <MoveMTCStageToContainer  container="{pick_object}"  stage="{allow_collisions_table}" />
-            <GeometryMsgsVector3Stamped  frame_id="{reference_frame}" vector="0,0,1" vector3_stamped="{tcp_translate}"/>
-            <CreateMTCMoveRelativeTranslate  stage_name="lift object"
-                                             stage="{lift_object}"
-                                             group="{arm_group_name}"
-                                             solver="{cartesian}"
-                                             ik_frame="{ik_frame}"
-                                             direction="{tcp_translate}"
-                                             min_distance="0.01"
-                                             max_distance="0.1" />
-            <MoveMTCStageToContainer  container="{pick_object}" stage="{lift_object}" />
-            <ForbidCollisionPairs  stage_name="forbid collisions -> object,support"
-                                          stage="{forbid_collisions_table}"
-                                          first="{object_name}"
-                                          second="table" />
-            <GetMTCRawStage  stage="{forbid_collisions_table}" raw_stage="{raw_forbid_collisions_table}}"  /> 
-            <MoveMTCStageToContainer  container="{pick_object}"  stage="{forbid_collisions_table}" />    
-            <MoveMTCContainerToParentContainer parent_container="{task_container}"  child_container="{pick_object}" />
-            <CreateMTCConnect  stage_name="Move To Place"
-                               timeout="5.0"
-                               group="{arm_group_name}"
-                               planner="{rrt_connect}"
-                               stage="{connectPlace}" />
-            <MoveMTCStageToContainer  container="{task_container}" stage="{connectPlace}" />
-            <CreateMTCSerialContainer  container_name="place object"
-                                       serial_container="{place_object}" />
-            <GeometryMsgsVector3Stamped  frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate}"/>
-            <CreateMTCMoveRelativeTranslate  stage_name="lower object"
-                                             stage="{lower_object}"
-                                             group="{arm_group_name}"
-                                             solver="{cartesian}"
-                                             ik_frame="{ik_frame}"
-                                             direction="{tcp_translate}"
-                                             min_distance="0.03"
-                                             max_distance="0.13" />
-            <MoveMTCStageToContainer  container="{place_object}" stage="{lower_object}" />
- */
-
-//
-
-//
-
-//
-//             <CreateMTCSerialContainer  container_name="place object"
-//                                        serial_container="{place_object}" />
-//             <GeometryMsgsVector3Stamped  frame_id="{reference_frame}" vector="0,0,-1" vector3_stamped="{tcp_translate}"/>
-//             <CreateMTCMoveRelativeTranslate  stage_name="lower object"
-//                                              stage="{lower_object}"
-//                                              group="{arm_group_name}"
-//                                              solver="{cartesian}"
-//                                              ik_frame="{ik_frame}"
-//                                              direction="{tcp_translate}"
-//                                              min_distance="0.03"
-//                                              max_distance="0.13" />
-//             <MoveMTCStageToContainer  container="{place_object}" stage="{lower_object}" />
diff --git a/examples/bt_joint_interpolation_solver.cpp b/examples/bt_joint_interpolation_solver.cpp
index a2b38a3..5deea21 100644
--- a/examples/bt_joint_interpolation_solver.cpp
+++ b/examples/bt_joint_interpolation_solver.cpp
@@ -47,12 +47,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCJointInterpolation>("CreateMTCJointInterpolation");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
@@ -76,11 +76,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_move_relative.cpp b/examples/bt_move_relative.cpp
index c00e360..a21d65b 100644
--- a/examples/bt_move_relative.cpp
+++ b/examples/bt_move_relative.cpp
@@ -74,12 +74,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
@@ -107,11 +107,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_move_to.cpp b/examples/bt_move_to.cpp
index 85926d2..1bfe823 100644
--- a/examples/bt_move_to.cpp
+++ b/examples/bt_move_to.cpp
@@ -84,11 +84,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
+
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
   factory.registerNodeType<CreateMTCCurrentState>("CreateMTCCurrentState");
@@ -116,12 +117,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_pick_place.cpp b/examples/bt_pick_place.cpp
index 54b3bd0..08be98d 100644
--- a/examples/bt_pick_place.cpp
+++ b/examples/bt_pick_place.cpp
@@ -299,12 +299,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<GeometryMsgsPose>("GeometryMsgsPose");
   factory.registerNodeType<GeometryMsgsPoseStamped>("GeometryMsgsPoseStamped");
   factory.registerNodeType<GeometryMsgsVector3Stamped>("GeometryMsgsVector3Stamped");
@@ -357,12 +357,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::cout << "======================" << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 
diff --git a/examples/bt_serial_container.cpp b/examples/bt_serial_container.cpp
index fba0458..987d086 100644
--- a/examples/bt_serial_container.cpp
+++ b/examples/bt_serial_container.cpp
@@ -64,12 +64,12 @@ static const char* xml_text = R"(
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "test_behavior_tree");
+  ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
 
   BehaviorTreeFactory factory;
-
   factory.registerNodeType<InitializeMTCTask>("InitializeMTCTask");
   factory.registerNodeType<CreateMTCPipelinePlanner>("CreateMTCPipelinePlanner");
   factory.registerNodeType<CreateMTCSerialContainer>("CreateMTCSerialContainer");
@@ -97,11 +97,17 @@ int main(int argc, char** argv)
   BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
 
   // Gives the user time to connect to Groot2
-  int wait_time = 5000;
-  std::cout << "Waiting " << wait_time << " msec for connection with Groot2...\n\n"
-            << std::endl;
-  std::this_thread::sleep_for(std::chrono::milliseconds(wait_time));
+  int delay_ms;
+  pnh.param<int>("delay_ms", delay_ms, 0.0);
+  if(delay_ms)
+  {
+    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
+              << std::endl;
+    std::cout << "======================" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+  }
 
+  // Start ticking the Tree
   std::cout << "Starting Behavior Tree" << std::endl;
   std::cout << "======================" << std::endl;
 

From 98d1315a3292c8d4f12c61f7941f48fe30ac9fec Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 11:08:17 -0500
Subject: [PATCH 08/15] Use same stoping criteria as the pickplace demo

---
 examples/bt_pick_place.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/bt_pick_place.cpp b/examples/bt_pick_place.cpp
index 08be98d..296c8a0 100644
--- a/examples/bt_pick_place.cpp
+++ b/examples/bt_pick_place.cpp
@@ -287,7 +287,7 @@ static const char* xml_text = R"(
             <MoveMTCStageToTask child="{stage_move_to_home}" parent="{mtc_task}" />
 
             <!-- Plan the Task -->
-            <PlanMTCTask task="{mtc_task}" max_solutions="5" />
+            <PlanMTCTask task="{mtc_task}" max_solutions="10" />
         </Sequence>
      </BehaviorTree>
 

From 0f5c717d60f04e618bd08bcf1e16a1a46831b2d0 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 11:29:24 -0500
Subject: [PATCH 09/15] Rename demo nodes

---
 examples/bt_allow_collisions.cpp           | 2 +-
 examples/bt_compute_ik.cpp                 | 2 +-
 examples/bt_create_collision_object.cpp    | 2 +-
 examples/bt_demo.cpp                       | 2 +-
 examples/bt_generate_place_pose.cpp        | 2 +-
 examples/bt_joint_interpolation_solver.cpp | 2 +-
 examples/bt_move_relative.cpp              | 2 +-
 examples/bt_move_to.cpp                    | 2 +-
 examples/bt_pick_place.cpp                 | 2 +-
 examples/bt_serial_container.cpp           | 2 +-
 10 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/examples/bt_allow_collisions.cpp b/examples/bt_allow_collisions.cpp
index fede627..7faad5c 100644
--- a/examples/bt_allow_collisions.cpp
+++ b/examples/bt_allow_collisions.cpp
@@ -90,7 +90,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_compute_ik.cpp b/examples/bt_compute_ik.cpp
index 4489c8a..bcbc28b 100644
--- a/examples/bt_compute_ik.cpp
+++ b/examples/bt_compute_ik.cpp
@@ -138,7 +138,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_create_collision_object.cpp b/examples/bt_create_collision_object.cpp
index 3dbff87..649b4b1 100644
--- a/examples/bt_create_collision_object.cpp
+++ b/examples/bt_create_collision_object.cpp
@@ -56,7 +56,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_demo.cpp b/examples/bt_demo.cpp
index 5a155b2..1798a08 100644
--- a/examples/bt_demo.cpp
+++ b/examples/bt_demo.cpp
@@ -32,7 +32,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_generate_place_pose.cpp b/examples/bt_generate_place_pose.cpp
index 1dcee7f..1e34aa4 100644
--- a/examples/bt_generate_place_pose.cpp
+++ b/examples/bt_generate_place_pose.cpp
@@ -212,7 +212,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_joint_interpolation_solver.cpp b/examples/bt_joint_interpolation_solver.cpp
index 5deea21..0823da5 100644
--- a/examples/bt_joint_interpolation_solver.cpp
+++ b/examples/bt_joint_interpolation_solver.cpp
@@ -46,7 +46,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_move_relative.cpp b/examples/bt_move_relative.cpp
index a21d65b..ba836cc 100644
--- a/examples/bt_move_relative.cpp
+++ b/examples/bt_move_relative.cpp
@@ -73,7 +73,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_move_to.cpp b/examples/bt_move_to.cpp
index 1bfe823..7a185c9 100644
--- a/examples/bt_move_to.cpp
+++ b/examples/bt_move_to.cpp
@@ -83,7 +83,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_pick_place.cpp b/examples/bt_pick_place.cpp
index 296c8a0..3b7517f 100644
--- a/examples/bt_pick_place.cpp
+++ b/examples/bt_pick_place.cpp
@@ -298,7 +298,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);
diff --git a/examples/bt_serial_container.cpp b/examples/bt_serial_container.cpp
index 987d086..b1969ee 100644
--- a/examples/bt_serial_container.cpp
+++ b/examples/bt_serial_container.cpp
@@ -63,7 +63,7 @@ static const char* xml_text = R"(
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "test_behavior_tree");
+  ros::init(argc, argv, "bt_mtc_demo");
   ros::NodeHandle pnh("~");
 
   ros::AsyncSpinner spinner(1);

From 9fab5da9c48461b84dfac24929279f205f6c8c3c Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 11:33:51 -0500
Subject: [PATCH 10/15] Add MTC demo launcher into this package

No need to remap the task solution topic manually.
---
 config/bt_mtc.rviz | 297 +++++++++++++++++++++++++++++++++++++++++++++
 launch/demo.launch |  13 ++
 2 files changed, 310 insertions(+)
 create mode 100644 config/bt_mtc.rviz
 create mode 100644 launch/demo.launch

diff --git a/config/bt_mtc.rviz b/config/bt_mtc.rviz
new file mode 100644
index 0000000..104a9b2
--- /dev/null
+++ b/config/bt_mtc.rviz
@@ -0,0 +1,297 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 84
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Motion Planning Tasks1
+      Splitter Ratio: 0.5393258333206177
+    Tree Height: 899
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: moveit_task_constructor/Motion Planning Tasks
+    Global Settings:
+      Task View Settings:
+        Old task handling: Keep
+        Show Computation Times: true
+        Task Expansion: All Expanded
+    Name: Motion Planning Tasks
+    Tasks View:
+      property_splitter:
+        - 919
+        - 0
+      solution_sorting:
+        column: 1
+        order: 0
+      solutions_splitter:
+        - 316
+        - 102
+      solutions_view_columns: ~
+      tasks_view_columns:
+        - 143
+        - 57
+        - 57
+        - 57
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+    - Class: moveit_task_constructor/Motion Planning Tasks
+      Enabled: true
+      Interrupt Display: false
+      Loop Animation: false
+      Markers:
+        All at once?: false
+        Value: true
+      Name: Motion Planning Tasks
+      Robot:
+        Fixed Robot Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Use Fixed Robot Color: false
+        Value: ""
+      Robot Description: robot_description
+      Scene:
+        Attached Body Color: 150; 50; 150
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Value: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Show Trail: false
+      State Display Time: REALTIME
+      Task Solution Topic: /bt_mtc_demo/solution
+      Tasks:
+        task pipeline: 1
+      Trail Step Size: 1
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.7952697277069092
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.18215136229991913
+        Y: 0.0822998434305191
+        Z: 0.374462366104126
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.48479723930358887
+      Target Frame: panda_link0
+      Yaw: 5.5999436378479
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1163
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Motion Planning Tasks:
+    collapsed: false
+  Motion Planning Tasks - Slider:
+    collapsed: false
+  QMainWindow State: 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
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 1920
+  Y: 0
diff --git a/launch/demo.launch b/launch/demo.launch
new file mode 100644
index 0000000..45e825c
--- /dev/null
+++ b/launch/demo.launch
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load panda demo with MTC's execution capability -->
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/demo.launch">
+    <arg name="use_rviz" value="false"/>
+  </include>
+  <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
+  <!-- Run rviz with prepared config -->
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="rviz_config" value="$(find behaviortree_mtc)/config/bt_mtc.rviz" />
+  </include>
+</launch>

From f6b445bab8aee3fad1abac97ad92797d5ff97ae2 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 11:41:58 -0500
Subject: [PATCH 11/15] Minimum MTC version compatibility

---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index 384da12..53a3f83 100644
--- a/README.md
+++ b/README.md
@@ -4,5 +4,7 @@
 
 This project is compatible with version 4.X of [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP).
 
+It is recommanded that the [Moveit Task Constructor](https://github.com/moveit/moveit_task_constructor) library commit version `>=` [fdc06c3](https://github.com/moveit/moveit_task_constructor/commit/fdc06c3b9105dadec2f44ee3e4b3133986945e86). This is to ensure that one can preempt a plan gracefully.
+
 ## Roadmap
 We plan to release version `1.0.0` by the end of summer 2024. Check the [roadmap](https://github.com/captain-yoshi/BehaviorTree.MTC/issues/15) for more details.

From 1f6882c2a7f257333029277076bff2b9f3d9adc7 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 11:42:24 -0500
Subject: [PATCH 12/15] Bump v1.0.0 release date

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 53a3f83..b4e9fd0 100644
--- a/README.md
+++ b/README.md
@@ -7,4 +7,4 @@ This project is compatible with version 4.X of [BehaviorTree.CPP](https://github
 It is recommanded that the [Moveit Task Constructor](https://github.com/moveit/moveit_task_constructor) library commit version `>=` [fdc06c3](https://github.com/moveit/moveit_task_constructor/commit/fdc06c3b9105dadec2f44ee3e4b3133986945e86). This is to ensure that one can preempt a plan gracefully.
 
 ## Roadmap
-We plan to release version `1.0.0` by the end of summer 2024. Check the [roadmap](https://github.com/captain-yoshi/BehaviorTree.MTC/issues/15) for more details.
+We plan to release version `1.0.0` by the end of Christmas 2024. Check the [roadmap](https://github.com/captain-yoshi/BehaviorTree.MTC/issues/15) for more details.

From d861eb853cd6a78457a6dde92c634461d70f6ac9 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 21 Nov 2024 11:44:06 -0500
Subject: [PATCH 13/15] Howto documentation

---
 README.md | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/README.md b/README.md
index b4e9fd0..f0cbe20 100644
--- a/README.md
+++ b/README.md
@@ -6,5 +6,24 @@ This project is compatible with version 4.X of [BehaviorTree.CPP](https://github
 
 It is recommanded that the [Moveit Task Constructor](https://github.com/moveit/moveit_task_constructor) library commit version `>=` [fdc06c3](https://github.com/moveit/moveit_task_constructor/commit/fdc06c3b9105dadec2f44ee3e4b3133986945e86). This is to ensure that one can preempt a plan gracefully.
 
+## Demo
+Check the available [examples](./examples).
+
+Running the Pick and Place MTC demo using BT's:
+
+``` sh
+# In one terminal
+$ roslaunch behaviortree_mtc demo.launch
+# ALTERNATE: Use the one supplied with MTC
+#            Don't forget to change the task solution topic
+
+# In another terminal
+$ rosrun behaviortree_mtc bt_pick_place
+
+
+# OPTIONAL: Connect from Groot2 to visualize the BT's on port 1667
+$ rosrun behaviortree_mtc bt_pick_place _delay_ms:=5000
+```
+
 ## Roadmap
 We plan to release version `1.0.0` by the end of Christmas 2024. Check the [roadmap](https://github.com/captain-yoshi/BehaviorTree.MTC/issues/15) for more details.

From 6b96246501af6b46e1f66d399b9b641a9440f2b8 Mon Sep 17 00:00:00 2001
From: DaniGarciaLopez <d.garcialopez@hotmail.com>
Date: Wed, 11 Dec 2024 23:17:15 +0100
Subject: [PATCH 14/15] Rolling port

---
 CMakeLists.txt                      |  5 +++++
 launch/run.launch.py                | 31 +++++++++++++++++++++++++++++
 src/create_mtc_cartesian_path.cpp   | 13 ++++++------
 src/create_mtc_pipeline_planner.cpp |  7 +++----
 4 files changed, 46 insertions(+), 10 deletions(-)
 create mode 100644 launch/run.launch.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6ff7ae3..69f06a7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -87,6 +87,11 @@ install(
   DESTINATION include
 )
 
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 install(TARGETS 
   bt_allow_collisions
   bt_create_collision_object
diff --git a/launch/run.launch.py b/launch/run.launch.py
new file mode 100644
index 0000000..93baeea
--- /dev/null
+++ b/launch/run.launch.py
@@ -0,0 +1,31 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .to_moveit_configs()
+    )
+
+    node = Node(
+        package="behaviortree_mtc",
+        executable="bt_demo",
+        output="screen",
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+            moveit_config.planning_pipelines,
+            os.path.join(get_package_share_directory("moveit_task_constructor_demo"), "config", "panda_config.yaml"),
+        ],
+    )
+
+    return LaunchDescription([node])
\ No newline at end of file
diff --git a/src/create_mtc_cartesian_path.cpp b/src/create_mtc_cartesian_path.cpp
index 5a621c3..0267279 100644
--- a/src/create_mtc_cartesian_path.cpp
+++ b/src/create_mtc_cartesian_path.cpp
@@ -18,23 +18,24 @@ BT::NodeStatus CreateMTCCartesianPath::tick()
   double max_velocity_scaling_factor;
   double max_acceleration_scaling_factor;
   double step_size;
-  double jump_treshold;
   double min_fraction;
+  moveit::core::CartesianPrecision precision;
 
   if(!getInput("max_velocity_scaling_factor", max_velocity_scaling_factor) ||
      !getInput("max_acceleration_scaling_factor", max_acceleration_scaling_factor) ||
      !getInput("step_size", step_size) ||
      !getInput("min_fraction", min_fraction) ||
-     !getInput("jump_threshold", jump_treshold))
+     !getInput("precision", precision))
     return NodeStatus::FAILURE;
 
-  //build solver
+  // Build solver
   auto solver = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
   solver->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
   solver->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
   solver->setStepSize(step_size);
-  solver->setJumpThreshold(jump_treshold);
   solver->setMinFraction(min_fraction);
+  solver->setPrecision(precision);
+
   // Upcast to base class
   moveit::task_constructor::solvers::PlannerInterfacePtr base_solver = solver;
 
@@ -47,9 +48,9 @@ BT::PortsList CreateMTCCartesianPath::providedPorts()
   return {
     BT::InputPort<double>("max_velocity_scaling_factor", 0.1, "scale down max velocity by this factor"),
     BT::InputPort<double>("max_acceleration_scaling_factor", 0.1, "scale down max acceleration by this factor"),
-    BT::InputPort<double>("step_size", 0.01, "step size between consecutive waypoints"),
     BT::InputPort<double>("min_fraction", 1.0, "fraction of motion required for success"),
-    BT::InputPort<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"),
+    BT::InputPort<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision({ .translational = 0.001, .rotational = 0.01, .max_resolution = 1e-5 }), "linear and rotational precision"),
+    BT::InputPort<double>("step_size", 0.01, "step size between consecutive waypoints"),
     BT::OutputPort<moveit::task_constructor::solvers::PlannerInterfacePtr>("solver", "{solver}", "Planner interface using pipeline motion solver"),
   };
 }
diff --git a/src/create_mtc_pipeline_planner.cpp b/src/create_mtc_pipeline_planner.cpp
index 88c901e..7d77650 100644
--- a/src/create_mtc_pipeline_planner.cpp
+++ b/src/create_mtc_pipeline_planner.cpp
@@ -42,8 +42,7 @@ BT::NodeStatus CreateMTCPipelinePlanner::tick()
      !getInput("num_planning_attempts", num_planning_attemps))
     return NodeStatus::FAILURE;
   //build solver
-  auto solver = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(node_, pipeline_id);
-  solver->setPlannerId(planner_id);
+  auto solver = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(node_, pipeline_id, planner_id);
   solver->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
   solver->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
   solver->setProperty("goal_joint_tolerance", goal_joint_tolerance);
@@ -68,8 +67,8 @@ BT::PortsList CreateMTCPipelinePlanner::providedPorts()
     BT::InputPort<double>("goal_joint_tolerance", "1e-4", "tolerance for reaching joint goals"),
     BT::InputPort<double>("goal_position_tolerance", "1e-4", "tolerance for reaching position goals"),
     BT::InputPort<double>("goal_orientation_tolerance", "1e-4", "tolerance for reaching orientation goals"),
-    BT::InputPort<bool>("display_motion_plans", false, "publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC),
-    BT::InputPort<bool>("publish_planning_requests", false, "publish motion planning requests on topic " + planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC),
+    BT::InputPort<bool>("display_motion_plans", false, "publish generated solutions on topic "),
+    BT::InputPort<bool>("publish_planning_requests", false, "publish motion planning requests on topic "),
     BT::InputPort<std::string>("planner_id"),
     BT::InputPort<std::string>("pipeline_id"),
     BT::InputPort<uint>("num_planning_attempts", "1u", "number of planning attempts"),

From e91fa824f98cc25a586f36b588d8a16c177c67d0 Mon Sep 17 00:00:00 2001
From: DaniGarciaLopez <d.garcialopez@hotmail.com>
Date: Sun, 15 Dec 2024 17:33:12 +0100
Subject: [PATCH 15/15] Add behaviortree_ros2 to package.xml

---
 package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/package.xml b/package.xml
index 964b8af..0b0c076 100644
--- a/package.xml
+++ b/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>behaviortree_cpp</depend>
+  <depend>behaviortree_ros2</depend>
   <depend>geometry_msgs</depend>
   <depend>moveit_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>