From 67123e00d09c6b37d0726cdebb7e17702bdf018a Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Mon, 6 Feb 2023 22:35:06 +0100
Subject: [PATCH] insert

---
 .vscode/c_cpp_properties.json | 2 +-
 src/bt/execution.cpp          | 8 ++++++--
 src/impl/moveit_mediator.cpp  | 5 ++++-
 3 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 473dc3f..f9513ef 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -32,7 +32,7 @@
       "intelliSenseMode": "gcc-x64",
       "compilerPath": "/usr/bin/gcc",
       "cStandard": "gnu11",
-      "cppStandard": "c++14"
+      "cppStandard": "c++17"
     }
   ],
   "version": 4
diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp
index eb95195..e502f16 100644
--- a/src/bt/execution.cpp
+++ b/src/bt/execution.cpp
@@ -22,7 +22,9 @@ BT::NodeStatus Execution::tick() {
 
 BT::NodeStatus Execution::onStart(){
     if (it_ != ets_.solution.sub_trajectory.end()){
-        executions_->insert(std::pair<std::string, moveit_msgs::RobotTrajectory>(mr_reference_->name(), it_->trajectory));
+        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());
         it_++;
         return BT::NodeStatus::RUNNING;
     } else {
@@ -34,7 +36,9 @@ 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(std::pair<std::string, moveit_msgs::RobotTrajectory>(mr_reference_->name(), it_->trajectory));
+        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());
         it_++;
         return BT::NodeStatus::RUNNING;
     } else {
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index 032dbf0..ea29805 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -909,6 +909,7 @@ void Moveit_mediator::task_planner(){
 		status = tree.tickRoot();
 
 		std::vector<std::thread> th;
+		
 		for(auto& exec : executions_){
 			for (int i = 0; i < robots_.size(); i++){
 				if (exec.first == robots_[i]->name()){
@@ -917,10 +918,12 @@ void Moveit_mediator::task_planner(){
 				}
 			}
 		}
-
+		
 		for(auto& t : th){
 			if(t.joinable()) t.join();
 		}
+
+		
 	}
 
 
-- 
GitLab