insert

.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

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 {

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()){
@@ -921,6 +922,8 @@ void Moveit_mediator::task_planner(){
 		for(auto& t : th){
 			if(t.joinable()) t.join();
 		}
+
+		
 	}