From fefe8e54db3c7aeac9aff189f02a77bebae0a45a Mon Sep 17 00:00:00 2001
From: Stephen Phillips <stphillips@clearpathrobotics.com>
Date: Wed, 31 Jul 2024 06:56:21 -0400
Subject: [PATCH] Add: Message definitions for odometry covariance watchdog

Signed-off-by: Stephen Phillips <stphillips@clearpathrobotics.com>
---
 .../clearpath_safety_msgs/CMakeLists.txt                     | 3 +++
 .../msg/WatchdogConfigOdomCovariance.msg                     | 4 ++++
 .../clearpath_safety_msgs/srv/AddOdomCovarianceWatchdog.srv  | 5 +++++
 .../srv/UpdateOdomCovarianceWatchdog.srv                     | 5 +++++
 4 files changed, 17 insertions(+)
 create mode 100644 clearpath_outdoornav_msgs/clearpath_safety_msgs/msg/WatchdogConfigOdomCovariance.msg
 create mode 100644 clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/AddOdomCovarianceWatchdog.srv
 create mode 100644 clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/UpdateOdomCovarianceWatchdog.srv

diff --git a/clearpath_outdoornav_msgs/clearpath_safety_msgs/CMakeLists.txt b/clearpath_outdoornav_msgs/clearpath_safety_msgs/CMakeLists.txt
index 5d48fd9..0d1be8c 100644
--- a/clearpath_outdoornav_msgs/clearpath_safety_msgs/CMakeLists.txt
+++ b/clearpath_outdoornav_msgs/clearpath_safety_msgs/CMakeLists.txt
@@ -21,6 +21,7 @@ add_message_files(
     WatchdogConfigNodeletStatus.msg
     WatchdogConfigNodeStatus.msg
     WatchdogConfigNumPoints.msg
+    WatchdogConfigOdomCovariance.msg
     WatchdogConfigRtkFix.msg
     WatchdogConfigTopicData.msg
     WatchdogStatus.msg
@@ -35,6 +36,7 @@ add_service_files(
     AddNodeletStatusWatchdog.srv
     AddNodeStatusWatchdog.srv
     AddNumPointsWatchdog.srv
+    AddOdomCovarianceWatchdog.srv
     AddRtkFixWatchdog.srv
     AddTopicDataWatchdog.srv
     RemoveWatchdog.srv
@@ -44,6 +46,7 @@ add_service_files(
     UpdateNodeletStatusWatchdog.srv
     UpdateNodeStatusWatchdog.srv
     UpdateNumPointsWatchdog.srv
+    UpdateOdomCovarianceWatchdog.srv
     UpdateRtkFixWatchdog.srv
     UpdateTopicDataWatchdog.srv
   )
diff --git a/clearpath_outdoornav_msgs/clearpath_safety_msgs/msg/WatchdogConfigOdomCovariance.msg b/clearpath_outdoornav_msgs/clearpath_safety_msgs/msg/WatchdogConfigOdomCovariance.msg
new file mode 100644
index 0000000..e911f30
--- /dev/null
+++ b/clearpath_outdoornav_msgs/clearpath_safety_msgs/msg/WatchdogConfigOdomCovariance.msg
@@ -0,0 +1,4 @@
+WatchdogConfigCommon base_config
+string topic          # Odometry topic
+float32 xy_threshold
+float32 yaw_threshold
diff --git a/clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/AddOdomCovarianceWatchdog.srv b/clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/AddOdomCovarianceWatchdog.srv
new file mode 100644
index 0000000..04100b9
--- /dev/null
+++ b/clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/AddOdomCovarianceWatchdog.srv
@@ -0,0 +1,5 @@
+
+WatchdogConfigOdomCovariance config
+
+---
+bool success
diff --git a/clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/UpdateOdomCovarianceWatchdog.srv b/clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/UpdateOdomCovarianceWatchdog.srv
new file mode 100644
index 0000000..9a50948
--- /dev/null
+++ b/clearpath_outdoornav_msgs/clearpath_safety_msgs/srv/UpdateOdomCovarianceWatchdog.srv
@@ -0,0 +1,5 @@
+
+WatchdogConfigOdomCovariance new_config
+string current_name     # used as a key to find the watchdog in the list of watchdogs
+---
+bool success