Groot now in parallel

include/bt/execution.h

@@ -23,13 +23,13 @@ class Execution : public BT::StatefulActionNode{
     moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_;
     Moveit_robot* mr_reference_;
     ros::Publisher* publisher_reference_;
-    std::map<std::string, moveit_msgs::RobotTrajectory>* executions_;
+    std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions_;
     std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_;
 
 
     public:
     Execution(const std::string& name, const BT::NodeConfiguration& config);
-    void init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions_, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets);
+    void init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions_, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets);
 
     inline static BT::PortsList providedPorts() { return {}; }
 

include/impl/moveit_mediator.h

@@ -77,7 +77,7 @@ class Moveit_mediator : public Abstract_mediator{
 
     std::map<std::string, std::vector<uint8_t>> acm_;
     std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;
-    std::map<std::string, moveit_msgs::RobotTrajectory> executions_;
+    std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>> executions_;
 
 
 
@@ -95,18 +95,19 @@ class Moveit_mediator : public Abstract_mediator{
     void status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status);
 
 
-    void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
+    void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr);
     void merge_acm(moveit_msgs::PlanningScene& in);
 
     void task_planner();
     void publish_tables();
-    void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
 
     void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt);
+    void manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps);
+
     //void parallel_exec();
 
     moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
-    inline std::map<std::string, moveit_msgs::RobotTrajectory>& executions(){return executions_;};
+    inline std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>& executions(){return executions_;};
     inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
 
     inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};

src/bt/execution.cpp

@@ -22,9 +22,8 @@ BT::NodeStatus Execution::tick() {
 
 BT::NodeStatus Execution::onStart(){
     if (it_ != ets_.solution.sub_trajectory.end()){
-        executions_->insert_or_assign(mr_reference_->name(), it_->trajectory);
-        ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size());
-		ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
+        std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
+        executions_->insert_or_assign(mr_reference_->name(), pair);
         it_++;
         return BT::NodeStatus::RUNNING;
     } else {
@@ -36,9 +35,8 @@ BT::NodeStatus Execution::onStart(){
     // this method until it return something different from RUNNING
 BT::NodeStatus Execution::onRunning(){
     if (it_ != ets_.solution.sub_trajectory.end()){
-        executions_->insert_or_assign(mr_reference_->name(), it_->trajectory);
-        ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size());
-		ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
+        std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
+        executions_->insert_or_assign(mr_reference_->name(), pair);
         it_++;
         return BT::NodeStatus::RUNNING;
     } else {
@@ -49,7 +47,7 @@ BT::NodeStatus Execution::onRunning(){
     // callback to execute if the action was aborted by another node
 void Execution::onHalted(){}
 
-void Execution::init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
+void Execution::init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
     mr_reference_= mr_reference;
     publisher_reference_ = publisher_reference;
     ets_ = ets;