mavros: fix run on Galactic

Signed-off-by: Vladimir Ermakov <[email protected]>
alpogant · Jun 20, 2021 · 164f3a2 · 164f3a2
1 parent f9dce35
commit 164f3a2
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 15 deletions.
diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
@@ -61,6 +61,12 @@ include_directories(
 ## Check if the datasets are installed
 include(CheckGeographicLibDatasets)
 
+if(rclcpp_VERSION VERSION_LESS 9.0.0)
+  add_definitions(
+    -DUSE_OLD_DECLARE_PARAMETER
+  )
+endif()
+
 # [[[cog:
 # import mavros_cog
 # ]]]

diff --git a/mavros/include/mavros/plugin.hpp b/mavros/include/mavros/plugin.hpp
@@ -226,12 +226,16 @@ class Plugin : public std::enable_shared_from_this<Plugin>
   )
   {
     node_watch_parameters[name] = cb;
+
+#ifdef USE_OLD_DECLARE_PARAMETER
     // NOTE(vooon): for Foxy:
     return node->declare_parameter(
       name,
       rclcpp::ParameterValue(), parameter_descriptor, ignore_override);
-    // NOTE(vooon): for master
-    // return node->declare_parameter<ParameterT>(name, parameter_descriptor, ignore_override);
+#else
+    // NOTE(vooon): for Galactic:
+    return node->declare_parameter<ParameterT>(name, parameter_descriptor, ignore_override);
+#endif
   }
 
   template<typename ParameterT>

diff --git a/mavros/src/plugins/param.cpp b/mavros/src/plugins/param.cpp
@@ -370,6 +370,7 @@ class Parameter
       "CMD_TOTAL",
       "CMD_INDEX",
       "LOG_LASTFILE",
+      "MIS_TOTAL",
       "FENCE_TOTAL",
       "FORMAT_VERSION"
     };
@@ -983,6 +984,7 @@ class ParamPlugin : public plugin::Plugin
         res->success = false;
         return;
       }
+      lock.unlock();
     }
 
     if (Parameter::check_exclude_param_id(req->param_id) && !req->force_set) {

diff --git a/mavros/src/plugins/waypoint.cpp b/mavros/src/plugins/waypoint.cpp
@@ -34,8 +34,12 @@ class WaypointPlugin : public plugin::MissionBase
 {
 public:
   explicit WaypointPlugin(plugin::UASPtr uas_)
-  : MissionBase(uas_, "mission")
+  : MissionBase(uas_, "mission"),
+    enable_partial_push_auto(true)
   {
+    rcl_interfaces::msg::ParameterDescriptor desc_pp{};
+    desc_pp.dynamic_typing = true;
+
     enable_node_watch_parameters();
 
     // NOTE(vooon): I'm not quite sure that this option would work with mavros router
@@ -49,16 +53,25 @@ class WaypointPlugin : public plugin::MissionBase
         use_mission_item_int = p.as_bool();
       });
 
-    node_declate_and_watch_parameter<bool>(
-      "enable_partial_push", [&](const rclcpp::Parameter & p) {
+    node_declate_and_watch_parameter(
+      "enable_partial_push", 2, [&](const rclcpp::Parameter & p) {
         RCLCPP_DEBUG_STREAM(get_logger(), log_prefix << ": enable_partial_push = " << p);
 
-        if (p.get_type() != rclcpp::PARAMETER_BOOL) {
-          return;
+        if (p.get_type() == rclcpp::PARAMETER_INTEGER) {
+          auto v = p.as_int();
+
+          enable_partial_push_auto = v >= 2;
+          if (enable_partial_push_auto) {
+            enable_partial_push = detect_partial_push();
+          } else {
+            enable_partial_push = v != 0;
+          }
         }
 
-        enable_partial_push = p.as_bool();
-      });
+        if (p.get_type() == rclcpp::PARAMETER_BOOL) {
+          enable_partial_push = p.as_bool();
+        }
+      }, desc_pp);
 
     auto wp_qos = rclcpp::QoS(10).transient_local();
 
@@ -94,6 +107,8 @@ class WaypointPlugin : public plugin::MissionBase
   rclcpp::Service<mavros_msgs::srv::WaypointClear>::SharedPtr clear_srv;
   rclcpp::Service<mavros_msgs::srv::WaypointSetCurrent>::SharedPtr set_cur_srv;
 
+  bool enable_partial_push_auto;
+
   /* -*- mid-level helpers -*- */
 
   // Acts when capabilities of the fcu are changed
@@ -120,14 +135,11 @@ class WaypointPlugin : public plugin::MissionBase
     if (connected) {
       schedule_pull(BOOTUP_TIME);
 
-      const auto key = "enable_partial_push";
-      rclcpp::Parameter p;
-      if (!node->get_parameter(key, p) || p.get_type() == rclcpp::PARAMETER_NOT_SET) {
-        bool new_state = uas->is_ardupilotmega();
+      if (enable_partial_push_auto) {
+        enable_partial_push = detect_partial_push();
 
         RCLCPP_INFO_STREAM(
-          get_logger(), log_prefix << ": detected enable_partial_push: " << new_state);
-        node->set_parameter(rclcpp::Parameter(key, new_state));
+          get_logger(), log_prefix << ": detected enable_partial_push: " << enable_partial_push);
       }
     } else if (schedule_timer) {
       schedule_timer->cancel();
@@ -159,6 +171,11 @@ class WaypointPlugin : public plugin::MissionBase
     wp_reached_pub->publish(wr);
   }
 
+  bool detect_partial_push()
+  {
+    return uas->is_ardupilotmega();
+  }
+
   /* -*- ROS callbacks -*- */
 
   void pull_cb(