From 8a0e96870891a7f12bd9b2ffd13121b9bc83fca0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A4=D1=80=D1=8B=D0=BC=D0=B0=D0=BD?= Date: Sun, 16 Jun 2024 20:51:03 +0000 Subject: [PATCH] Clean shutdown of UAS node Prohibit copying and moving of UAS Removed startup_delay_timer --- mavros/include/mavros/mavros_uas.hpp | 19 ++- mavros/src/lib/mavros_uas.cpp | 207 +++++++++++++-------------- mavros/test/test_uas.cpp | 1 - 3 files changed, 116 insertions(+), 111 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.hpp b/mavros/include/mavros/mavros_uas.hpp index fb24c25db..a44a38756 100644 --- a/mavros/include/mavros/mavros_uas.hpp +++ b/mavros/include/mavros/mavros_uas.hpp @@ -251,6 +251,16 @@ class UAS : public rclcpp::Node const std::string & uas_url_ = "/uas1", uint8_t target_system_ = 1, uint8_t target_component_ = 1); + /** + * @brief Prohibit @a UAS copying, because plugins hold raw pointers to @a UAS. + */ + UAS(UAS const &) = delete; + + /** + * @brief Prohibit @a UAS moving, because plugins hold raw pointers to @a UAS. + */ + UAS(UAS &&) = delete; + ~UAS() = default; /** @@ -580,11 +590,7 @@ class UAS : public rclcpp::Node StrV plugin_denylist; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr set_parameters_handle_ptr; - rclcpp::TimerBase::SharedPtr startup_delay_timer; - // XXX(vooon): we have to use own executor because Node::create_sub_node() doesn't work for us. - using thread_ptr = std::unique_ptr>; - thread_ptr exec_spin_thd; // rclcpp::executors::MultiThreadedExecutor exec; UASExecutor exec; @@ -616,6 +622,11 @@ class UAS : public rclcpp::Node rclcpp::Subscription::SharedPtr source; // FCU -> UAS rclcpp::Publisher::SharedPtr sink; // UAS -> FCU + // XXX(vooon): we have to use own executor because Node::create_sub_node() doesn't work for us. + // The executor thread is the last thing to initialize, and it must be the first thing to destroy. + using thread_ptr = std::unique_ptr>; + thread_ptr exec_spin_thd; + //! initialize connection to the Router void connect_to_router(); diff --git a/mavros/src/lib/mavros_uas.cpp b/mavros/src/lib/mavros_uas.cpp index 424d92404..e609ff148 100644 --- a/mavros/src/lib/mavros_uas.cpp +++ b/mavros/src/lib/mavros_uas.cpp @@ -85,119 +85,114 @@ UAS::UAS( this->declare_parameter("map_frame_id", map_frame_id); // NOTE: we can add_plugin() in constructor because it does not need shared_from_this() - startup_delay_timer = this->create_wall_timer( - 10ms, [this]() { - startup_delay_timer->cancel(); - - std::string fcu_protocol; - int tgt_system, tgt_component; - this->get_parameter("uas_url", uas_url); - this->get_parameter("fcu_protocol", fcu_protocol); - this->get_parameter("system_id", source_system); - this->get_parameter("component_id", source_component); - this->get_parameter("target_system_id", tgt_system); - this->get_parameter("target_component_id", tgt_component); - this->get_parameter("plugin_allowlist", plugin_allowlist); - this->get_parameter("plugin_denylist", plugin_denylist); - this->get_parameter("base_link_frame_id", base_link_frame_id); - this->get_parameter("odom_frame_id", odom_frame_id); - this->get_parameter("map_frame_id", map_frame_id); - - exec_spin_thd = thread_ptr( - new std::thread( - [this]() { - utils::set_this_thread_name("uas-exec/%d.%d", source_system, source_component); - auto lg = this->get_logger(); - - RCLCPP_INFO( - lg, "UAS Executor started, threads: %zu", - this->exec.get_number_of_threads()); - this->exec.spin(); - RCLCPP_WARN(lg, "UAS Executor terminated"); - }), - [this](std::thread * t) { - this->exec.cancel(); - t->join(); - delete t; - }); - - // setup diag - diagnostic_updater.setHardwareID(utils::format("uas://%s", uas_url.c_str())); - diagnostic_updater.add("MAVROS UAS", this, &UAS::diag_run); - - // setup uas link - if (fcu_protocol == "v1.0") { - set_protocol_version(mavconn::Protocol::V10); - } else if (fcu_protocol == "v2.0") { - set_protocol_version(mavconn::Protocol::V20); - } else { - RCLCPP_WARN( - get_logger(), - "Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v2.0.", - fcu_protocol.c_str()); - set_protocol_version(mavconn::Protocol::V20); - } + std::string fcu_protocol; + int tgt_system, tgt_component; + this->get_parameter("uas_url", uas_url); + this->get_parameter("fcu_protocol", fcu_protocol); + this->get_parameter("system_id", source_system); + this->get_parameter("component_id", source_component); + this->get_parameter("target_system_id", tgt_system); + this->get_parameter("target_component_id", tgt_component); + this->get_parameter("plugin_allowlist", plugin_allowlist); + this->get_parameter("plugin_denylist", plugin_denylist); + this->get_parameter("base_link_frame_id", base_link_frame_id); + this->get_parameter("odom_frame_id", odom_frame_id); + this->get_parameter("map_frame_id", map_frame_id); + + // setup diag + diagnostic_updater.setHardwareID(utils::format("uas://%s", uas_url.c_str())); + diagnostic_updater.add("MAVROS UAS", this, &UAS::diag_run); + + // setup uas link + if (fcu_protocol == "v1.0") { + set_protocol_version(mavconn::Protocol::V10); + } else if (fcu_protocol == "v2.0") { + set_protocol_version(mavconn::Protocol::V20); + } else { + RCLCPP_WARN( + get_logger(), + "Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v2.0.", + fcu_protocol.c_str()); + set_protocol_version(mavconn::Protocol::V20); + } - // setup source and target - set_tgt(tgt_system, tgt_component); + // setup source and target + set_tgt(tgt_system, tgt_component); - add_connection_change_handler( - std::bind( - &UAS::log_connect_change, this, - std::placeholders::_1)); + add_connection_change_handler( + std::bind( + &UAS::log_connect_change, this, + std::placeholders::_1)); - // prepare plugin lists - // issue #257 2: assume that all plugins blacklisted - if (plugin_denylist.empty() && !plugin_allowlist.empty()) { - plugin_denylist.emplace_back("*"); - } + // prepare plugin lists + // issue #257 2: assume that all plugins blacklisted + if (plugin_denylist.empty() && !plugin_allowlist.empty()) { + plugin_denylist.emplace_back("*"); + } - for (auto & name : plugin_factory_loader.getDeclaredClasses()) { - add_plugin(name); - } + for (auto & name : plugin_factory_loader.getDeclaredClasses()) { + add_plugin(name); + } - connect_to_router(); - - // Publish helper TFs used for frame transformation in the odometry plugin - { - std::string base_link_frd = base_link_frame_id + "_frd"; - std::string odom_ned = odom_frame_id + "_ned"; - std::string map_ned = map_frame_id + "_ned"; - std::vector transform_vector; - add_static_transform( - map_frame_id, map_ned, Eigen::Affine3d( - ftf::quaternion_from_rpy( - M_PI, 0, - M_PI_2)), - transform_vector); - add_static_transform( - odom_frame_id, odom_ned, Eigen::Affine3d( - ftf::quaternion_from_rpy( - M_PI, 0, - M_PI_2)), - transform_vector); - add_static_transform( - base_link_frame_id, base_link_frd, - Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector); - - tf2_static_broadcaster.sendTransform(transform_vector); - } + connect_to_router(); + + // Publish helper TFs used for frame transformation in the odometry plugin + { + std::string base_link_frd = base_link_frame_id + "_frd"; + std::string odom_ned = odom_frame_id + "_ned"; + std::string map_ned = map_frame_id + "_ned"; + std::vector transform_vector; + add_static_transform( + map_frame_id, map_ned, Eigen::Affine3d( + ftf::quaternion_from_rpy( + M_PI, 0, + M_PI_2)), + transform_vector); + add_static_transform( + odom_frame_id, odom_ned, Eigen::Affine3d( + ftf::quaternion_from_rpy( + M_PI, 0, + M_PI_2)), + transform_vector); + add_static_transform( + base_link_frame_id, base_link_frd, + Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector); + + tf2_static_broadcaster.sendTransform(transform_vector); + } - std::stringstream ss; - for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) { - ss << " " << s; - } + std::stringstream ss; + for (auto & s : mavconn::MAVConnInterface::get_known_dialects()) { + ss << " " << s; + } - RCLCPP_INFO( - get_logger(), "Built-in SIMD instructions: %s", - Eigen::SimdInstructionSetsInUse()); - RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version); - RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str()); - RCLCPP_INFO( - get_logger(), "MAVROS UAS via %s started. MY ID %u.%u, TARGET ID %u.%u", - uas_url.c_str(), - source_system, source_component, - target_system, target_component); + RCLCPP_INFO( + get_logger(), "Built-in SIMD instructions: %s", + Eigen::SimdInstructionSetsInUse()); + RCLCPP_INFO(get_logger(), "Built-in MAVLink package version: %s", mavlink::version); + RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str()); + RCLCPP_INFO( + get_logger(), "MAVROS UAS via %s started. MY ID %u.%u, TARGET ID %u.%u", + uas_url.c_str(), + source_system, source_component, + target_system, target_component); + + exec_spin_thd = thread_ptr( + new std::thread( + [this]() { + utils::set_this_thread_name("uas-exec/%d.%d", source_system, source_component); + auto lg = this->get_logger(); + + RCLCPP_INFO( + lg, "UAS Executor started, threads: %zu", + this->exec.get_number_of_threads()); + this->exec.spin(); + RCLCPP_WARN(lg, "UAS Executor terminated"); + }), + [this](std::thread * t) { + this->exec.cancel(); + t->join(); + delete t; }); } diff --git a/mavros/test/test_uas.cpp b/mavros/test/test_uas.cpp index e23f378cc..539fffd0c 100644 --- a/mavros/test/test_uas.cpp +++ b/mavros/test/test_uas.cpp @@ -127,7 +127,6 @@ class TestUAS : public ::testing::Test MockUAS::SharedPtr create_node() { auto uas = std::make_shared("test_mavros_uas"); - uas->startup_delay_timer->cancel(); return uas; }