diff --git a/cmake/DARTFindDependencies.cmake b/cmake/DARTFindDependencies.cmake index 51071ce08504c..dfd7ba9f43868 100644 --- a/cmake/DARTFindDependencies.cmake +++ b/cmake/DARTFindDependencies.cmake @@ -106,6 +106,9 @@ else() ) endif() +# Taskflow +dart_find_package(Taskflow) + #======================= # Optional dependencies #======================= diff --git a/cmake/DARTFindTaskflow.cmake b/cmake/DARTFindTaskflow.cmake new file mode 100644 index 0000000000000..ddc3eeda29e70 --- /dev/null +++ b/cmake/DARTFindTaskflow.cmake @@ -0,0 +1,9 @@ +# Copyright (c) 2011-2024, The DART development contributors +# All rights reserved. +# +# The list of contributors can be found at: +# https://github.com/dartsim/dart/blob/main/LICENSE +# +# This file is provided under the "BSD-style" License + +find_package(Taskflow) diff --git a/dart/CMakeLists.txt b/dart/CMakeLists.txt index 3c547e94389f2..24e607bd44c38 100644 --- a/dart/CMakeLists.txt +++ b/dart/CMakeLists.txt @@ -127,6 +127,7 @@ target_link_libraries(dart Eigen3::Eigen fcl assimp + Taskflow::Taskflow ) if(TARGET fmt::fmt-header-only) target_link_libraries(dart PUBLIC fmt::fmt-header-only) @@ -214,7 +215,7 @@ endif() add_component_targets(${PROJECT_NAME} dart dart) add_component_dependencies(${PROJECT_NAME} dart external-odelcpsolver) add_component_dependency_packages(${PROJECT_NAME} dart - assimp Eigen3 fcl fmt + assimp Eigen3 fcl fmt Taskflow ) if(TARGET octomap) add_component_dependency_packages(${PROJECT_NAME} dart octomap) diff --git a/dart/common/ThreadPool.cpp b/dart/common/ThreadPool.cpp deleted file mode 100644 index fb1a5ea0d88a9..0000000000000 --- a/dart/common/ThreadPool.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Copyright (c) 2011-2024, The DART development contributors - * All rights reserved. - * - * The list of contributors can be found at: - * https://github.com/dartsim/dart/blob/main/LICENSE - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "dart/common/ThreadPool.hpp" - -#include "dart/common/Console.hpp" -#include "dart/common/Logging.hpp" -#include "dart/common/Macros.hpp" - -namespace dart::common { - -//============================================================================== - -} // namespace dart::common diff --git a/dart/common/ThreadPool.hpp b/dart/common/ThreadPool.hpp deleted file mode 100644 index 5132a760a465a..0000000000000 --- a/dart/common/ThreadPool.hpp +++ /dev/null @@ -1,113 +0,0 @@ -/* - * Copyright (c) 2011-2024, The DART development contributors - * All rights reserved. - * - * The list of contributors can be found at: - * https://github.com/dartsim/dart/blob/main/LICENSE - * - * This file is provided under the following "BSD-style" License: - * Redistribution and use in source and binary forms, with or - * without modification, are permitted provided that the following - * conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, - * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF - * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace dart::common { - -class ThreadPool -{ -public: - ThreadPool(size_t threads) : stop(false) - { - for (size_t i = 0; i < threads; ++i) - workers.emplace_back([this] { - while (true) { - std::function task; - - { - std::unique_lock lock(this->queueMutex); - this->condition.wait( - lock, [this] { return this->stop || !this->tasks.empty(); }); - if (this->stop && this->tasks.empty()) - return; - task = std::move(this->tasks.front()); - this->tasks.pop(); - } - - task(); - } - }); - } - - template - auto enqueue(F&& f, Args&&... args) - -> std::future::type> - { - using return_type = typename std::result_of::type; - - auto task = std::make_shared>( - std::bind(std::forward(f), std::forward(args)...)); - - std::future res = task->get_future(); - { - std::unique_lock lock(queueMutex); - - // don't allow enqueueing after stopping the pool - if (stop) - throw std::runtime_error("enqueue on stopped ThreadPool"); - - tasks.emplace([task]() { (*task)(); }); - } - condition.notify_one(); - return res; - } - - ~ThreadPool() - { - { - std::unique_lock lock(queueMutex); - stop = true; - } - condition.notify_all(); - for (std::thread& worker : workers) - worker.join(); - } - -private: - std::vector workers; - std::queue> tasks; - - std::mutex queueMutex; - std::condition_variable condition; - bool stop; -}; - -} // namespace dart::common diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 7a4a8c7595e9f..4fe840684f0a1 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -34,7 +34,6 @@ #include "dart/common/Logging.hpp" #include "dart/common/Profile.hpp" -#include "dart/common/ThreadPool.hpp" #include "dart/constraint/ConstraintBase.hpp" #include "dart/constraint/DantzigBoxedLcpSolver.hpp" #include "dart/constraint/PgsBoxedLcpSolver.hpp" @@ -230,7 +229,7 @@ DART_SUPPRESS_DEPRECATED_END //============================================================================== BoxedLcpConstraintSolver::BoxedLcpConstraintSolver(const Config& config) - : ConstraintSolver(), mConfig(config), mThreadPool(4) + : ConstraintSolver(), mConfig(config), mExecutor(4) { // Empty } @@ -250,7 +249,7 @@ DART_SUPPRESS_DEPRECATED_END DART_SUPPRESS_DEPRECATED_BEGIN BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( BoxedLcpSolverPtr boxedLcpSolver, BoxedLcpSolverPtr secondaryBoxedLcpSolver) - : ConstraintSolver(), mThreadPool(4) + : ConstraintSolver(), mExecutor(4) { if (boxedLcpSolver) { setBoxedLcpSolver(std::move(boxedLcpSolver)); @@ -620,18 +619,10 @@ void BoxedLcpConstraintSolver::solveConstrainedGroups() { DART_PROFILE_SCOPED_N("Solve problems"); - - std::vector> results; - - for (auto i = 0u; i < numGroups; ++i) { - results.emplace_back(mThreadPool.enqueue( - [this, i]() { solveBoxedLcp(mProblems[i], mConstrainedGroups[i]); })); - } - - // Wait for all tasks to complete - for (auto&& result : results) { - result.get(); - } + mTaskflow.for_each_index(0, numGroups, 1, [&](int i) { + solveBoxedLcp(mProblems[i], mConstrainedGroups[i]); + }); + mExecutor.run(mTaskflow).wait(); } } diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 1661bd59a1241..c57f7b797e1e1 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -35,7 +35,8 @@ #include #include -#include + +#include namespace dart { namespace constraint { @@ -249,7 +250,8 @@ class BoxedLcpConstraintSolver : public ConstraintSolver std::vector mProblems; - common::ThreadPool mThreadPool; + tf::Executor mExecutor; + tf::Taskflow mTaskflow; }; } // namespace constraint