Merge branch 'master' into ros2

* master: 1.18.0 update changelog sys_status.cpp: improve timeout code sys_status.cpp: Add a SYS_STATUS message publisher [camera plugin] Fix image_index and capture_result not properly filled Fix missing semi-colon GPS_STATUS Plugin: Fill in available messages for ROS1 legacy
mavlink · Mar 3, 2024 · 1652486 · 1652486
2 parents ef8de4c + 7f1a8c7
commit 1652486
Show file tree

Hide file tree

Showing 14 changed files with 100 additions and 16 deletions.
diff --git a/libmavconn/CHANGELOG.rst b/libmavconn/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package libmavconn
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.18.0 (2024-03-03)
+-------------------
+
 2.6.0 (2023-09-09)
 ------------------
 * fix ament_cpplint

diff --git a/mavros/CHANGELOG.rst b/mavros/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package mavros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.18.0 (2024-03-03)
+-------------------
+* sys_status.cpp: improve timeout code
+* sys_status.cpp: Add a SYS_STATUS message publisher
+* Contributors: Dr.-Ing. Amilcar do Carmo Lucas
+
 2.6.0 (2023-09-09)
 ------------------
 * fix build warnings tf2_eigen.h

diff --git a/mavros/src/lib/mavros_uas.cpp b/mavros/src/lib/mavros_uas.cpp
@@ -160,9 +160,9 @@ UAS::UAS(
 
       // 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::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(

diff --git a/mavros/src/mavros_node.cpp b/mavros/src/mavros_node.cpp
@@ -100,7 +100,6 @@ int main(int argc, char * argv[])
       uas_params.emplace_back("map_frame_id", map_frame_id);
     }
     uas_node->set_parameters(uas_params);
-
   }
 
   exec.spin();

diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp
@@ -28,6 +28,7 @@
 #include "mavros/plugin_filter.hpp"
 
 #include "mavros_msgs/msg/state.hpp"
+#include "mavros_msgs/msg/sys_status.hpp"
 #include "mavros_msgs/msg/estimator_status.hpp"
 #include "mavros_msgs/msg/extended_state.hpp"
 #include "mavros_msgs/srv/stream_rate.hpp"
@@ -571,6 +572,8 @@ class SystemStatusPlugin : public plugin::Plugin
       "state", state_qos);
     extended_state_pub = node->create_publisher<mavros_msgs::msg::ExtendedState>(
       "extended_state", state_qos);
+    sys_status_pub = node->create_publisher<mavros_msgs::msg::SysStatus>(
+      "sys_status", state_qos);
     estimator_status_pub = node->create_publisher<mavros_msgs::msg::EstimatorStatus>(
       "estimator_status", state_qos);
     batt_pub = node->create_publisher<BatteryMsg>("battery", sensor_qos);
@@ -645,6 +648,7 @@ class SystemStatusPlugin : public plugin::Plugin
 
   rclcpp::Publisher<mavros_msgs::msg::State>::SharedPtr state_pub;
   rclcpp::Publisher<mavros_msgs::msg::ExtendedState>::SharedPtr extended_state_pub;
+  rclcpp::Publisher<mavros_msgs::msg::SysStatus>::SharedPtr sys_status_pub;
   rclcpp::Publisher<mavros_msgs::msg::EstimatorStatus>::SharedPtr estimator_status_pub;
   rclcpp::Publisher<BatteryMsg>::SharedPtr batt_pub;
 
@@ -748,13 +752,16 @@ class SystemStatusPlugin : public plugin::Plugin
    *
    * @param[in] severity
    */
-  void process_event_normal(uint8_t severity, uint32_t px4_id, const std::array<uint8_t, 40>& arguments)
+  void process_event_normal(
+    uint8_t severity, uint32_t px4_id, const std::array<uint8_t,
+    40> & arguments)
   {
     using mavlink::common::MAV_SEVERITY;
 
     std::string arg_str = "-";
-    for (const uint8_t& arg : arguments)
+    for (const uint8_t & arg : arguments) {
       arg_str += std::to_string(arg) + "-";
+    }
 
     switch (severity) {
       // [[[cog:
@@ -773,7 +780,9 @@ class SystemStatusPlugin : public plugin::Plugin
       case enum_value(MAV_SEVERITY::ALERT):
       case enum_value(MAV_SEVERITY::CRITICAL):
       case enum_value(MAV_SEVERITY::ERROR):
-        RCLCPP_ERROR_STREAM(node->get_logger(), "FCU: EVENT " << px4_id << " with args " << arg_str);
+        RCLCPP_ERROR_STREAM(
+          node->get_logger(),
+          "FCU: EVENT " << px4_id << " with args " << arg_str);
         break;
       case enum_value(MAV_SEVERITY::WARNING):
       case enum_value(MAV_SEVERITY::NOTICE):
@@ -783,11 +792,15 @@ class SystemStatusPlugin : public plugin::Plugin
         RCLCPP_INFO_STREAM(node->get_logger(), "FCU: EVENT " << px4_id << " with args " << arg_str);
         break;
       case enum_value(MAV_SEVERITY::DEBUG):
-        RCLCPP_DEBUG_STREAM(node->get_logger(), "FCU: EVENT " << px4_id << " with args " << arg_str);
+        RCLCPP_DEBUG_STREAM(
+          node->get_logger(),
+          "FCU: EVENT " << px4_id << " with args " << arg_str);
         break;
       // [[[end]]] (checksum: 83f5eab6a8989f95de46d2a95387304c)
       default:
-        RCLCPP_WARN_STREAM(node->get_logger(), "FCU: UNK(" << +severity << "): EVENT " << px4_id << " with args " << arg_str);
+        RCLCPP_WARN_STREAM(
+          node->get_logger(),
+          "FCU: UNK(" << +severity << "): EVENT " << px4_id << " with args " << arg_str);
         break;
     }
   }
@@ -959,6 +972,25 @@ class SystemStatusPlugin : public plugin::Plugin
     battery_voltage = volt;
     sys_diag.set(stat);
 
+    auto sys_status = mavros_msgs::msg::SysStatus();
+    sys_status.header.stamp = node->now();
+    sys_status.sensors_present = stat.onboard_control_sensors_present;
+    sys_status.sensors_enabled = stat.onboard_control_sensors_enabled;
+    sys_status.sensors_health = stat.onboard_control_sensors_health;
+
+    sys_status.load = stat.load;
+    sys_status.voltage_battery = stat.voltage_battery;
+    sys_status.current_battery = stat.current_battery;
+    sys_status.battery_remaining = stat.battery_remaining;
+    sys_status.drop_rate_comm = stat.drop_rate_comm;
+    sys_status.errors_comm = stat.errors_comm;
+    sys_status.errors_count1 = stat.errors_count1;
+    sys_status.errors_count2 = stat.errors_count2;
+    sys_status.errors_count3 = stat.errors_count3;
+    sys_status.errors_count4 = stat.errors_count4;
+
+    sys_status_pub->publish(sys_status);
+
     if (has_battery_status0) {
       return;
     }

diff --git a/mavros/tools/cogall.sh b/mavros/tools/cogall.sh
@@ -2,4 +2,6 @@
 
 set -x
 
+export PYTHONPATH=.
+
 fgrep '[[[cog' --exclude 'cogall.sh' --exclude '*.md' --exclude '*.swp' -lr | python3 -m cogapp -cr @-
diff --git a/mavros_extras/CHANGELOG.rst b/mavros_extras/CHANGELOG.rst
@@ -2,6 +2,15 @@
 Changelog for package mavros_extras
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.18.0 (2024-03-03)
+-------------------
+* [camera plugin] Fix image_index and capture_result not properly filled
+* Fix missing semi-colon
+* GPS_STATUS Plugin: Fill in available messages for ROS1 legacy
+  Filled in available fields in GPS_RAW_INT & GPS2_RAW messages
+  p.s. seems GPS2_RAW more complete than original GPS_RAW_INT
+* Contributors: Beniamino Pozzan, Seunghwan Jo
+
 2.6.0 (2023-09-09)
 ------------------
 * switch to use tf2_eigen.hpp, but that drops support for EOL distros

diff --git a/mavros_extras/src/plugins/guided_target.cpp b/mavros_extras/src/plugins/guided_target.cpp
@@ -67,7 +67,8 @@ class GuidedTargetPlugin : public plugin::Plugin
 
     // Subscriber for global origin (aka map origin).
     gp_origin_sub = node->create_subscription<geographic_msgs::msg::GeoPointStamped>(
-      "global_position/gp_origin", rclcpp::SensorDataQoS(), std::bind(&GuidedTargetPlugin::gp_origin_cb, this, _1));
+      "global_position/gp_origin", rclcpp::SensorDataQoS(),
+      std::bind(&GuidedTargetPlugin::gp_origin_cb, this, _1));
   }
 
   Subscriptions get_subscriptions() override

diff --git a/mavros_extras/src/plugins/odom.cpp b/mavros_extras/src/plugins/odom.cpp
@@ -144,8 +144,12 @@ class OdometryPlugin : public plugin::Plugin
     Eigen::Affine3d tf_parent2parent_des;
     Eigen::Affine3d tf_child2child_des;
 
-    lookup_static_transform(fcu_odom_parent_id_des, fcu_odom_parent_id_des+"_ned", tf_parent2parent_des);
-    lookup_static_transform(fcu_odom_child_id_des, fcu_odom_child_id_des+"_frd", tf_child2child_des);
+    lookup_static_transform(
+      fcu_odom_parent_id_des, fcu_odom_parent_id_des + "_ned",
+      tf_parent2parent_des);
+    lookup_static_transform(
+      fcu_odom_child_id_des, fcu_odom_child_id_des + "_frd",
+      tf_child2child_des);
 
     //! Build 6x6 pose covariance matrix to be transformed and sent
     Matrix6d cov_pose = Matrix6d::Zero();
@@ -235,8 +239,12 @@ class OdometryPlugin : public plugin::Plugin
     Eigen::Affine3d tf_parent2parent_des;
     Eigen::Affine3d tf_child2child_des;
 
-    lookup_static_transform(odom->header.frame_id+"_ned", odom->header.frame_id, tf_parent2parent_des);
-    lookup_static_transform(odom->child_frame_id+"_frd", odom->child_frame_id, tf_child2child_des);
+    lookup_static_transform(
+      odom->header.frame_id + "_ned", odom->header.frame_id,
+      tf_parent2parent_des);
+    lookup_static_transform(
+      odom->child_frame_id + "_frd", odom->child_frame_id,
+      tf_child2child_des);
 
     //! Build 6x6 pose covariance matrix to be transformed and sent
     ftf::Covariance6d cov_pose = odom->pose.covariance;

diff --git a/mavros_msgs/CHANGELOG.rst b/mavros_msgs/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package mavros_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.18.0 (2024-03-03)
+-------------------
+* sys_status.cpp: Add a SYS_STATUS message publisher
+* Contributors: Dr.-Ing. Amilcar do Carmo Lucas
+
 2.6.0 (2023-09-09)
 ------------------
 * msgs: move generator code

diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt
@@ -85,6 +85,7 @@ set(msg_files
   msg/State.msg
   msg/StatusEvent.msg
   msg/StatusText.msg
+  msg/SysStatus.msg
   msg/TerrainReport.msg
   msg/Thrust.msg
   msg/TimesyncStatus.msg
@@ -97,7 +98,7 @@ set(msg_files
   msg/WaypointList.msg
   msg/WaypointReached.msg
   msg/WheelOdomStamped.msg
-  # [[[end]]] (checksum: d43b903630fd3ce3856c8484a2d1b709)
+  # [[[end]]] (checksum: 8a6c06289f2a7d9149b7309c1fe63463)
 )
 
 set(srv_files

diff --git a/mavros_msgs/msg/SysStatus.msg b/mavros_msgs/msg/SysStatus.msg
@@ -0,0 +1,15 @@
+std_msgs/Header header
+
+uint32 sensors_present
+uint32 sensors_enabled
+uint32 sensors_health
+uint16 load
+uint16 voltage_battery
+int16 current_battery
+int8 battery_remaining
+uint16 drop_rate_comm
+uint16 errors_comm
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4
diff --git a/test_mavros/CHANGELOG.rst b/test_mavros/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package test_mavros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.18.0 (2024-03-03)
+-------------------
+
 1.17.0 (2023-09-09)
 -------------------
 * Merge pull request `#1865 <https://github.com/mavlink/mavros/issues/1865>`_ from scoutdi/warnings

diff --git a/test_mavros/package.xml b/test_mavros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>test_mavros</name>
-  <version>1.17.0</version>
+  <version>1.18.0</version>
   <description>Tests for MAVROS package</description>
 
   <maintainer email="[email protected]">Vladimir Ermakov</maintainer>