diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 473dc3fcfc36899ca2e29b524ae01585630bc7de..f9513ef1ee7006509cce0faaca8d6212b32801d9 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 eb95195b812b63bfb9e82b1f6ff2c49e1c57a3b2..e502f162eab9be1a4e4974399f4f7e4cf88723ab 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 032dbf08ec3f56a4a1cea714a36f7794e38a23ef..ea29805702cefe929a54ca6128fb26edc50c229b 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(); } + + }