From 6ee2c17e314454dbcbd3f099bef22496e723a7f2 Mon Sep 17 00:00:00 2001 From: Nikola Jovicic Date: Thu, 23 Mar 2023 15:47:34 +0100 Subject: [PATCH] Create a new MutuallyExclusive callback group for every timer, use a multithreaded executor --- .../include/airsim_ros_wrapper.h | 5 ++++ ros2/src/airsim_ros_pkgs/src/airsim_node.cpp | 30 +++++-------------- .../src/airsim_ros_wrapper.cpp | 12 +++++--- 3 files changed, 20 insertions(+), 27 deletions(-) diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index a5126034b7..85a3b98203 100755 --- a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -348,6 +348,11 @@ class AirsimROSWrapper rclcpp::TimerBase::SharedPtr airsim_control_update_timer_; rclcpp::TimerBase::SharedPtr airsim_lidar_update_timer_; + /// Callback groups + std::vector airsim_img_callback_groups_; + rclcpp::CallbackGroup::SharedPtr airsim_control_callback_group_; + std::vector airsim_lidar_callback_groups_; + typedef std::pair, std::string> airsim_img_request_vehicle_name_pair; std::vector airsim_img_request_vehicle_name_pair_vec_; std::vector image_pub_vec_; diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp index 1003f8556a..693bdba034 100644 --- a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp @@ -1,35 +1,19 @@ #include #include "airsim_ros_wrapper.h" -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - std::shared_ptr nh = rclcpp::Node::make_shared("airsim_node", node_options); - std::shared_ptr nh_img = nh->create_sub_node("img"); - std::shared_ptr nh_lidar = nh->create_sub_node("lidar"); + std::shared_ptr nh = rclcpp::Node::make_shared("airsim_node", node_options); + std::shared_ptr nh_img = nh->create_sub_node("img"); + std::shared_ptr nh_lidar = nh->create_sub_node("lidar"); std::string host_ip; nh->get_parameter("host_ip", host_ip); AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, host_ip); - - if (airsim_ros_wrapper.is_used_img_timer_cb_queue_) { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(nh_img); - while (rclcpp::ok()) { - executor.spin(); - } - } - - if (airsim_ros_wrapper.is_used_lidar_timer_cb_queue_) { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(nh_lidar); - while (rclcpp::ok()) { - executor.spin(); - } - } - - rclcpp::spin(nh); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(nh); + executor.spin(); return 0; } \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index ce3f3390df..e0917b0eac 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -109,7 +109,8 @@ void AirsimROSWrapper::initialize_ros() nh_->declare_parameter("vehicle_name", rclcpp::ParameterValue("")); create_ros_pubs_from_settings_json(); - airsim_control_update_timer_ = nh_->create_wall_timer(std::chrono::duration(update_airsim_control_every_n_sec), std::bind(&AirsimROSWrapper::drone_state_timer_cb, this)); + airsim_control_callback_group_ = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + airsim_control_update_timer_ = nh_->create_wall_timer(std::chrono::duration(update_airsim_control_every_n_sec), std::bind(&AirsimROSWrapper::drone_state_timer_cb, this), airsim_control_callback_group_); } void AirsimROSWrapper::create_ros_pubs_from_settings_json() @@ -309,8 +310,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() if (!airsim_img_request_vehicle_name_pair_vec_.empty()) { double update_airsim_img_response_every_n_sec; nh_->get_parameter("update_airsim_img_response_every_n_sec", update_airsim_img_response_every_n_sec); - - airsim_img_response_timer_ = nh_img_->create_wall_timer(std::chrono::duration(update_airsim_img_response_every_n_sec), std::bind(&AirsimROSWrapper::img_response_timer_cb, this)); + auto cb = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + airsim_img_callback_groups_.push_back(cb); + airsim_img_response_timer_ = nh_img_->create_wall_timer(std::chrono::duration(update_airsim_img_response_every_n_sec), std::bind(&AirsimROSWrapper::img_response_timer_cb, this), cb); is_used_img_timer_cb_queue_ = true; } @@ -318,7 +320,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() if (lidar_cnt > 0) { double update_lidar_every_n_sec; nh_->get_parameter("update_lidar_every_n_sec", update_lidar_every_n_sec); - airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this)); + auto cb = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + airsim_lidar_callback_groups_.push_back(cb); + airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this), cb); is_used_lidar_timer_cb_queue_ = true; }