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