Skip to content

Commit

Permalink
Parse '--executor-type'
Browse files Browse the repository at this point in the history
Signed-off-by: Tim Clephas <[email protected]>
  • Loading branch information
Timple committed Jun 24, 2024
1 parent 2c75845 commit 83a9a1f
Showing 1 changed file with 34 additions and 20 deletions.
54 changes: 34 additions & 20 deletions rclcpp_components/src/component_container_isolated.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,46 +13,60 @@
// limitations under the License.

#include <memory>
#include <vector>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_components/component_manager_isolated.hpp"

int main(int argc, char * argv[])
{
/// Component container with dedicated single-threaded executors for each components.
/// Component container with dedicated executor for each component
rclcpp::init(argc, argv);

// parse arguments
bool use_multi_threaded_executor{false};
bool use_events_executor{false};
// valid entries: --executor-type single-threaded, --executor-type multi-threaded, --executor-type events
// --use-multi-threaded-executor and --use_multi_threaded_executor are kept for backward compatibility
std::vector<std::string> args = rclcpp::remove_ros_arguments(argc, argv);
for (auto & arg : args) {
if (arg == std::string("--use_multi_threaded_executor") ||
arg == std::string("--use-multi-threaded-executor"))

std::string executor_type = "single-threaded"; // default
for (size_t i = 0; i < args.size(); ++i) {
if (args[i] == "--executor-type") {
if (i + 1 < args.size()) {
executor_type = args[i + 1];
break;
}
} else if (
args[i] == "--use-multi-threaded-executor" || args[i] == "--use_multi_threaded_executor")
{
use_multi_threaded_executor = true;
} else if (arg == std::string("--use-events-executor")) {
use_events_executor = true;
executor_type = "multi-threaded";
}
}

// create executor and component manager
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
rclcpp::Node::SharedPtr node;
if (use_multi_threaded_executor) {
using ComponentManagerIsolated =
rclcpp_components::ComponentManagerIsolated<rclcpp::executors::MultiThreadedExecutor>;
std::shared_ptr<rclcpp::Executor> exec;
if (executor_type == "events") {
using executor = rclcpp::experimental::executors::EventsExecutor;
using ComponentManagerIsolated = rclcpp_components::ComponentManagerIsolated<executor>;
exec = std::make_shared<executor>();
node = std::make_shared<ComponentManagerIsolated>(exec);
} else if (use_events_executor) {
using ComponentManagerIsolated =
rclcpp_components::ComponentManagerIsolated<rclcpp::experimental::executors::EventsExecutor>;
} else if (executor_type == "multi-threaded") {
using executor = rclcpp::executors::MultiThreadedExecutor;
using ComponentManagerIsolated = rclcpp_components::ComponentManagerIsolated<executor>;
exec = std::make_shared<executor>();
node = std::make_shared<ComponentManagerIsolated>(exec);
} else {
using ComponentManagerIsolated =
rclcpp_components::ComponentManagerIsolated<rclcpp::executors::SingleThreadedExecutor>;
} else if (executor_type == "single-threaded") {
using executor = rclcpp::executors::SingleThreadedExecutor;
using ComponentManagerIsolated = rclcpp_components::ComponentManagerIsolated<executor>;
exec = std::make_shared<executor>();
node = std::make_shared<ComponentManagerIsolated>(exec);
} else {
std::cerr << "Invalid executor type: " << executor_type << std::endl;
return 1;
}

exec->add_node(node);
exec->spin();
}

0 comments on commit 83a9a1f

Please sign in to comment.