Fix resource management in UAS node

libmavconn/src/interface.cpp

@@ -99,7 +99,7 @@ void MAVConnInterface::iostat_rx_add(size_t bytes)
 }
 
 void MAVConnInterface::parse_buffer(
-  const char * pfx, uint8_t * buf, const size_t bufsize,
+  const char * pfx, uint8_t * buf, const size_t bufsize [[maybe_unused]],
   size_t bytes_received)
 {
   mavlink::mavlink_message_t message;

mavros/include/mavros/mavros_uas.hpp

@@ -161,7 +161,8 @@ class Data
   inline double geoid_to_ellipsoid_height(const T lla)
   {
     if (egm96_5) {
-      return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude);
+      return static_cast<int>(GeographicLib::Geoid::GEOIDTOELLIPSOID) *
+             (*egm96_5)(lla->latitude, lla->longitude);
     } else {
       return 0.0;
     }
@@ -181,7 +182,8 @@ class Data
   inline double ellipsoid_to_geoid_height(const T lla)
   {
     if (egm96_5) {
-      return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude);
+      return static_cast<int>(GeographicLib::Geoid::ELLIPSOIDTOGEOID) *
+             (*egm96_5)(lla->latitude, lla->longitude);
     } else {
       return 0.0;
     }
@@ -249,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;
 
   /**
@@ -578,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<std::thread, std::function<void (std::thread *)>>;
-  thread_ptr exec_spin_thd;
   // rclcpp::executors::MultiThreadedExecutor exec;
   UASExecutor exec;
 
@@ -614,6 +622,11 @@ class UAS : public rclcpp::Node
   rclcpp::Subscription<mavros_msgs::msg::Mavlink>::SharedPtr source;    // FCU -> UAS
   rclcpp::Publisher<mavros_msgs::msg::Mavlink>::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<std::thread, std::function<void (std::thread *)>>;
+  thread_ptr exec_spin_thd;
+
   //! initialize connection to the Router
   void connect_to_router();
 

mavros/include/mavros/plugin.hpp

@@ -44,7 +44,7 @@ namespace plugin
 {
 
 using mavros::uas::UAS;
-using UASPtr = std::shared_ptr<UAS>;
+using UASPtr = UAS *;
 using r_unique_lock = std::unique_lock<std::recursive_mutex>;
 using s_unique_lock = std::unique_lock<std::shared_timed_mutex>;
 using s_shared_lock = std::shared_lock<std::shared_timed_mutex>;
@@ -74,18 +74,9 @@ class Plugin : public std::enable_shared_from_this<Plugin>
   //! Subscriptions vector
   using Subscriptions = std::vector<HandlerInfo>;
 
-  explicit Plugin(UASPtr uas_)
-  : uas(uas_), node(std::dynamic_pointer_cast<rclcpp::Node>(uas_))
-  {}
-
   explicit Plugin(
-    UASPtr uas_, const std::string & subnode,
-    const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
-  : uas(uas_),
-    // node(std::dynamic_pointer_cast<rclcpp::Node>(uas_)->create_sub_node(subnode))  // https://github.com/ros2/rclcpp/issues/731
-    node(rclcpp::Node::make_shared(subnode,
-      std::dynamic_pointer_cast<rclcpp::Node>(uas_)->get_fully_qualified_name(), options))
-  {}
+    UASPtr uas, const std::string & subnode_name,
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
   virtual ~Plugin() = default;
 

mavros/include/mavros/plugin_filter.hpp

@@ -32,7 +32,7 @@ namespace filter
 {
 using mavros::plugin::Filter;
 using mavros::uas::UAS;
-using UASPtr = UAS::SharedPtr;
+using UASPtr = UAS *;
 using mavconn::Framing;
 
 

mavros/include/mavros/px4_custom_mode.hpp

@@ -100,7 +100,7 @@ union custom_mode {
   }
 
   constexpr custom_mode(uint8_t mm, uint8_t sm)
-  : mode{0, mm, sm}
+    : mode{0, mm, sm}
   {
   }
 };

mavros/include/mavros/setpoint_mixin.hpp

@@ -45,9 +45,9 @@ class SetPositionTargetLocalNEDMixin
   void set_position_target_local_ned(
     uint32_t time_boot_ms, uint8_t coordinate_frame,
     uint16_t type_mask,
-    Eigen::Vector3d p,
-    Eigen::Vector3d v,
-    Eigen::Vector3d af,
+    Eigen::Vector3d const & p,
+    Eigen::Vector3d const & v,
+    Eigen::Vector3d const & af,
     float yaw, float yaw_rate)
   {
     static_assert(

mavros/src/lib/mavros_uas.cpp

@@ -84,120 +84,115 @@ UAS::UAS(
   this->declare_parameter("odom_frame_id", odom_frame_id);
   this->declare_parameter("map_frame_id", map_frame_id);
 
-  // NOTE(vooon): we couldn't add_plugin() in constructor because it needs 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);
-      }
+  // NOTE: we can add_plugin() in constructor because it does not need shared_from_this()
+  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<geometry_msgs::msg::TransformStamped> 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<geometry_msgs::msg::TransformStamped> 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;
     });
 }
 
@@ -264,7 +259,7 @@ plugin::Plugin::SharedPtr UAS::create_plugin_instance(const std::string & pl_nam
   auto plugin_factory = plugin_factory_loader.createSharedInstance(pl_name);
 
   return
-    plugin_factory->create_plugin_instance(std::static_pointer_cast<UAS>(shared_from_this()));
+    plugin_factory->create_plugin_instance(this);
 }
 
 void UAS::add_plugin(const std::string & pl_name)

mavros/src/lib/plugin.cpp

@@ -16,7 +16,16 @@
 #include "mavros/mavros_uas.hpp"
 #include "mavros/plugin.hpp"
 
-using  mavros::plugin::Plugin;
+using mavros::plugin::Plugin;
+
+Plugin::Plugin(
+  UASPtr uas_, const std::string & subnode,
+  const rclcpp::NodeOptions & options)
+: uas(uas_),
+  // node(std::dynamic_pointer_cast<rclcpp::Node>(uas_)->create_sub_node(subnode))  // https://github.com/ros2/rclcpp/issues/731
+  node(rclcpp::Node::make_shared(subnode,
+    uas_->get_fully_qualified_name(), options))
+{}
 
 void Plugin::enable_connection_cb()
 {

mavros/src/plugins/home_position.cpp

@@ -130,7 +130,7 @@ class HomePositionPlugin : public plugin::Plugin
     hp.geo.latitude = home_position.latitude / 1E7;     // deg
     hp.geo.longitude = home_position.longitude / 1E7;   // deg
     hp.geo.altitude = home_position.altitude / 1E3 +
-      uas->data.geoid_to_ellipsoid_height(hp.geo);      // in meters
+      uas->data.geoid_to_ellipsoid_height(hp.geo);    // in meters
     hp.orientation = tf2::toMsg(q);
     hp.position = tf2::toMsg(pos);
     tf2::toMsg(hp_approach_enu, hp.approach);

mavros/src/plugins/setpoint_accel.cpp

@@ -75,8 +75,7 @@ class SetpointAccelerationPlugin : public plugin::Plugin,
   {
     using mavlink::common::MAV_FRAME;
 
-    bool send_force;
-    node->get_parameter("send_force", send_force);
+    bool const send_force = node->get_parameter("send_force").as_bool();
 
     /* Documentation start from bit 1 instead 0.
      * Ignore position and velocity vectors, yaw and yaw rate