From eae036fa9ba9760d30698faf048d5e62405db188 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 3 Mar 2024 07:57:38 +0000 Subject: [PATCH] Imported upstream version '1.18.0' of 'upstream' --- libmavconn/CHANGELOG.rst | 3 +++ libmavconn/package.xml | 2 +- mavros/CHANGELOG.rst | 6 +++++ mavros/package.xml | 2 +- mavros/src/plugins/sys_status.cpp | 28 ++++++++++++++++++++++-- mavros_extras/CHANGELOG.rst | 9 ++++++++ mavros_extras/package.xml | 2 +- mavros_extras/src/plugins/camera.cpp | 2 ++ mavros_extras/src/plugins/gps_status.cpp | 12 +++++----- mavros_msgs/CHANGELOG.rst | 5 +++++ mavros_msgs/CMakeLists.txt | 1 + mavros_msgs/msg/SysStatus.msg | 15 +++++++++++++ mavros_msgs/package.xml | 2 +- test_mavros/CHANGELOG.rst | 3 +++ test_mavros/package.xml | 2 +- 15 files changed, 82 insertions(+), 12 deletions(-) create mode 100644 mavros_msgs/msg/SysStatus.msg diff --git a/libmavconn/CHANGELOG.rst b/libmavconn/CHANGELOG.rst index a0b47ccd8..80c301474 100644 --- a/libmavconn/CHANGELOG.rst +++ b/libmavconn/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package libmavconn ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.18.0 (2024-03-03) +------------------- + 1.17.0 (2023-09-09) ------------------- * Merge pull request `#1865 `_ from scoutdi/warnings diff --git a/libmavconn/package.xml b/libmavconn/package.xml index 5be2fcb0a..311d0d166 100644 --- a/libmavconn/package.xml +++ b/libmavconn/package.xml @@ -1,7 +1,7 @@ libmavconn - 1.17.0 + 1.18.0 MAVLink communication library. This library provide unified connection handling classes diff --git a/mavros/CHANGELOG.rst b/mavros/CHANGELOG.rst index 96a0265e5..5d7c7b682 100644 --- 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 + 1.17.0 (2023-09-09) ------------------- * cog: regenerate all diff --git a/mavros/package.xml b/mavros/package.xml index 1f2df7e93..d7a30a6f6 100644 --- a/mavros/package.xml +++ b/mavros/package.xml @@ -1,7 +1,7 @@ mavros - 1.17.0 + 1.18.0 MAVROS -- MAVLink extendable communication node for ROS with proxy for Ground Control Station. diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index bfa5f116d..c85cbbeb8 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG @@ -394,11 +395,12 @@ class MemInfo : public diagnostic_updater::DiagnosticTask ros::Time last_rcd_; last_rcd_.fromNSec(last_rcd.load()); constexpr int timeout = 10.0; // seconds + const ros::Duration timeout_duration = ros::Duration(timeout); // summarize the results if (last_rcd_.isZero()) { stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not initialised"); - } else if (ros::Time::now().toSec() - last_rcd_.toSec() > timeout) { + } else if (ros::Time::now() - last_rcd_ > timeout_duration) { stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "Not received for more than " + std::to_string(timeout) + "s"); } else { if (freemem == UINT32_MAX) @@ -444,9 +446,10 @@ class HwStatus : public diagnostic_updater::DiagnosticTask { std::lock_guard lock(mutex); constexpr int timeout = 10.0; // seconds + const ros::Duration timeout_duration = ros::Duration(timeout); if (last_rcd.isZero()) { stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not initialised"); - } else if (ros::Time::now().toSec() - last_rcd.toSec() > timeout) { + } else if (ros::Time::now() - last_rcd > timeout_duration) { stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "Not received for more than " + std::to_string(timeout) + "s"); } else { if (vcc < 0) @@ -556,6 +559,7 @@ class SystemStatusPlugin : public plugin::PluginBase batt_pub = nh.advertise("battery", 10); estimator_status_pub = nh.advertise("estimator_status", 10); statustext_pub = nh.advertise("statustext/recv", 10); + sys_status_pub = nh.advertise("sys_status", 10); statustext_sub = nh.subscribe("statustext/send", 10, &SystemStatusPlugin::statustext_cb, this); rate_srv = nh.advertiseService("set_stream_rate", &SystemStatusPlugin::set_rate_cb, this); mode_srv = nh.advertiseService("set_mode", &SystemStatusPlugin::set_mode_cb, this); @@ -598,6 +602,7 @@ class SystemStatusPlugin : public plugin::PluginBase ros::Publisher batt_pub; ros::Publisher estimator_status_pub; ros::Publisher statustext_pub; + ros::Publisher sys_status_pub; ros::Subscriber statustext_sub; ros::ServiceServer rate_srv; ros::ServiceServer mode_srv; @@ -832,6 +837,25 @@ class SystemStatusPlugin : public plugin::PluginBase sys_diag.set(stat); + mavros_msgs::SysStatus sys_status; + sys_status.header.stamp = ros::Time::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_extras/CHANGELOG.rst b/mavros_extras/CHANGELOG.rst index 7cdad2b4e..45adc4dcf 100644 --- 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 + 1.17.0 (2023-09-09) ------------------- * Merge pull request `#1865 `_ from scoutdi/warnings diff --git a/mavros_extras/package.xml b/mavros_extras/package.xml index 884617102..a705e7ada 100644 --- a/mavros_extras/package.xml +++ b/mavros_extras/package.xml @@ -1,7 +1,7 @@ mavros_extras - 1.17.0 + 1.18.0 Extra nodes and plugins for MAVROS. diff --git a/mavros_extras/src/plugins/camera.cpp b/mavros_extras/src/plugins/camera.cpp index a18d85212..9ca08a9ce 100644 --- a/mavros_extras/src/plugins/camera.cpp +++ b/mavros_extras/src/plugins/camera.cpp @@ -76,6 +76,8 @@ class CameraPlugin : public plugin::PluginBase { ic->relative_alt = mo.relative_alt / 1E3; auto q = ftf::mavlink_to_quaternion(mo.q); tf::quaternionEigenToMsg(q, ic->orientation); + ic->image_index = mo.image_index; + ic->capture_result = mo.capture_result; ic->file_url = mavlink::to_string(mo.file_url); camera_image_captured_pub.publish(ic); diff --git a/mavros_extras/src/plugins/gps_status.cpp b/mavros_extras/src/plugins/gps_status.cpp index c06eb4248..cc4410c55 100644 --- a/mavros_extras/src/plugins/gps_status.cpp +++ b/mavros_extras/src/plugins/gps_status.cpp @@ -84,6 +84,7 @@ class GpsStatusPlugin : public plugin::PluginBase { ros_msg->hdg_acc = mav_msg.hdg_acc; ros_msg->dgps_numch = UINT8_MAX; // information not available in GPS_RAW_INT mavlink message ros_msg->dgps_age = UINT32_MAX;// information not available in GPS_RAW_INT mavlink message + ros_msg->yaw = mav_msg.yaw; gps1_raw_pub.publish(ros_msg); } @@ -103,13 +104,14 @@ class GpsStatusPlugin : public plugin::PluginBase { ros_msg->vel = mav_msg.vel; ros_msg->cog = mav_msg.cog; ros_msg->satellites_visible = mav_msg.satellites_visible; - ros_msg->alt_ellipsoid = INT32_MAX; // information not available in GPS2_RAW mavlink message - ros_msg->h_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message - ros_msg->v_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message - ros_msg->vel_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message - ros_msg->hdg_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message + ros_msg->alt_ellipsoid = mav_msg.alt_ellipsoid; + ros_msg->h_acc = mav_msg.h_acc; + ros_msg->v_acc = mav_msg.v_acc; + ros_msg->vel_acc = mav_msg.vel_acc; + ros_msg->hdg_acc = mav_msg.hdg_acc; ros_msg->dgps_numch = mav_msg.dgps_numch; ros_msg->dgps_age = mav_msg.dgps_age; + ros_msg->yaw = mav_msg.yaw; gps2_raw_pub.publish(ros_msg); } diff --git a/mavros_msgs/CHANGELOG.rst b/mavros_msgs/CHANGELOG.rst index 133409ded..a2b3709a4 100644 --- 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 + 1.17.0 (2023-09-09) ------------------- * cog: regenerate all diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index e2fa4e2f3..667bc5690 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -60,6 +60,7 @@ add_message_files( RTKBaseline.msg State.msg StatusText.msg + SysStatus.msg TerrainReport.msg Thrust.msg TimesyncStatus.msg diff --git a/mavros_msgs/msg/SysStatus.msg b/mavros_msgs/msg/SysStatus.msg new file mode 100644 index 000000000..551940a1b --- /dev/null +++ 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 \ No newline at end of file diff --git a/mavros_msgs/package.xml b/mavros_msgs/package.xml index e8ce7bd1c..0bc299b5e 100644 --- a/mavros_msgs/package.xml +++ b/mavros_msgs/package.xml @@ -1,7 +1,7 @@ mavros_msgs - 1.17.0 + 1.18.0 mavros_msgs defines messages for MAVROS. diff --git a/test_mavros/CHANGELOG.rst b/test_mavros/CHANGELOG.rst index 25b3b19cd..1c98394ef 100644 --- 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 `_ from scoutdi/warnings diff --git a/test_mavros/package.xml b/test_mavros/package.xml index dd813443f..c10efa19e 100644 --- a/test_mavros/package.xml +++ b/test_mavros/package.xml @@ -1,7 +1,7 @@ test_mavros - 1.17.0 + 1.18.0 Tests for MAVROS package Vladimir Ermakov