diff --git a/tesseract_monitoring/src/contact_monitor.cpp b/tesseract_monitoring/src/contact_monitor.cpp index 561ed970..6018d586 100644 --- a/tesseract_monitoring/src/contact_monitor.cpp +++ b/tesseract_monitoring/src/contact_monitor.cpp @@ -110,6 +110,7 @@ void ContactMonitor::computeCollisionReportThread() { boost::shared_ptr msg = nullptr; tesseract_collision::ContactResultMap contacts; + tesseract_collision::ContactResultVector contacts_vector; tesseract_msgs::ContactResultVector contacts_msg; std::string root_link; // Limit the lock @@ -152,6 +153,7 @@ void ContactMonitor::computeCollisionReportThread() current_joint_states_.reset(); contacts.clear(); + contacts_vector.clear(); contacts_msg.contacts.clear(); monitor_->environment().setState(msg->name, @@ -164,8 +166,7 @@ void ContactMonitor::computeCollisionReportThread() if (!contacts.empty()) { - tesseract_collision::ContactResultVector contacts_vector; - tesseract_collision::flattenMoveResults(std::move(contacts), contacts_vector); + contacts.flattenCopyResults(contacts_vector); contacts_msg.contacts.reserve(contacts_vector.size()); for (std::size_t i = 0; i < contacts_vector.size(); ++i) { @@ -240,7 +241,10 @@ bool ContactMonitor::callbackModifyTesseractEnv(tesseract_msgs::ModifyEnvironmen bool ContactMonitor::callbackComputeContactResultVector(tesseract_msgs::ComputeContactResultVectorRequest& request, tesseract_msgs::ComputeContactResultVectorResponse& response) { - tesseract_collision::ContactResultMap contact_results; + thread_local tesseract_collision::ContactResultMap contact_results; + thread_local tesseract_collision::ContactResultVector contacts_vector; + contact_results.clear(); + contacts_vector.clear(); monitor_->environment().setState( request.joint_states.name, @@ -254,8 +258,7 @@ bool ContactMonitor::callbackComputeContactResultVector(tesseract_msgs::ComputeC manager_->contactTest(contact_results, type_); } - tesseract_collision::ContactResultVector contacts_vector; - tesseract_collision::flattenMoveResults(std::move(contact_results), contacts_vector); + contact_results.flattenCopyResults(contacts_vector); response.collision_result.contacts.reserve(contacts_vector.size()); for (const auto& contact : contacts_vector) {