From bfc9bf845747f47e753b471be468f2284b30b7c2 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Thu, 9 Nov 2023 11:16:51 +0100
Subject: [PATCH 01/25] Add healthcheck

---
 Dockerfile                   | 40 +++++++++++++++++---
 healthcheck_localization.cpp | 37 ++++++++++++++++++
 healthcheck_localization.py  | 21 -----------
 healthcheck_navigation.cpp   | 28 ++++++++++++++
 healthcheck_slam.cpp         | 73 ++++++++++++++++++++++++++++++++++++
 healthcheck_slam.py          | 26 -------------
 6 files changed, 172 insertions(+), 53 deletions(-)
 create mode 100755 healthcheck_localization.cpp
 delete mode 100755 healthcheck_localization.py
 create mode 100644 healthcheck_navigation.cpp
 create mode 100755 healthcheck_slam.cpp
 delete mode 100755 healthcheck_slam.py

diff --git a/Dockerfile b/Dockerfile
index 86a3bfc..3e481e6 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -1,12 +1,12 @@
 ARG ROS_DISTRO=humble
 ARG PREFIX=
 
-FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core
+FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base
 
 SHELL ["/bin/bash", "-c"]
 
 ARG PREFIX
-ENV SLAM_MODE=localization
+ENV SLAM_MODE=
 ENV PREFIX_ENV=$PREFIX
 
 RUN apt update && apt upgrade -y && apt install -y \
@@ -19,11 +19,39 @@ RUN apt update && apt upgrade -y && apt install -y \
 
 COPY ./nav2_params /nav2_params
 
-COPY --chmod=755 healthcheck_* /
+# Create healthcheck package
+WORKDIR /ros2_ws
+
+RUN mkdir src && cd src/ && \
+    source /opt/ros/$ROS_DISTRO/setup.bash && \
+    ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp std_msgs && \
+    sed -i '/find_package(std_msgs REQUIRED)/a \
+            find_package(geometry_msgs REQUIRED)\n \
+            find_package(nav2_msgs REQUIRED)\n \
+            add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
+            ament_target_dependencies(healthcheck_localization rclcpp std_msgs geometry_msgs)\n \
+            add_executable(healthcheck_slam src/healthcheck_slam.cpp)\n \
+            ament_target_dependencies(healthcheck_slam rclcpp std_msgs nav2_msgs)\n \
+            add_executable(healthcheck_ src/healthcheck_navigation.cpp)\n \
+            ament_target_dependencies(healthcheck_ rclcpp std_msgs geometry_msgs)\n \
+            install(TARGETS \
+                healthcheck_localization \
+                healthcheck_slam  \
+                healthcheck_ \
+                DESTINATION lib/${PROJECT_NAME})' \
+            /ros2_ws/src/healthcheck_pkg/CMakeLists.txt
+
+COPY ./healthcheck_localization.cpp /ros2_ws/src/healthcheck_pkg/src/
+COPY ./healthcheck_slam.cpp /ros2_ws/src/healthcheck_pkg/src/
+COPY ./healthcheck_navigation.cpp /ros2_ws/src/healthcheck_pkg/src/
+
+# Build
+RUN source /opt/ros/$ROS_DISTRO/setup.bash && \
+    colcon build --event-handlers  console_direct+  --cmake-args  -DCMAKE_BUILD_TYPE=Release
 
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
-HEALTHCHECK --interval=10s --timeout=10s --start-period=5s --retries=6  \
-    CMD bash -c "source /opt/$([ -n "${PREFIX_ENV}" ] && echo "${PREFIX_ENV//-/}" || echo "ros")/$ROS_DISTRO/setup.bash && /healthcheck_$SLAM_MODE.py"
+HEALTHCHECK --interval=15s --timeout=5s --start-period=10s --retries=3 \
+    CMD bash -c "/ros_entrypoint.sh ros2 run healthcheck_pkg healthcheck_$SLAM_MODE"
 
-#tip: gathering logs from healthcheck: docker inspect b39 | jq '.[0].State.Health.Log'
\ No newline at end of file
+#tip: gathering logs from healthcheck: docker inspect b39 | jq '.[0].State.Health.Log'
diff --git a/healthcheck_localization.cpp b/healthcheck_localization.cpp
new file mode 100755
index 0000000..9128aaf
--- /dev/null
+++ b/healthcheck_localization.cpp
@@ -0,0 +1,37 @@
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "cstdlib"
+
+using namespace std::chrono_literals;
+
+#define TOPIC_NAME "/amcl_pose"
+#define TIMEOUT 2s
+
+int msg_received = EXIT_FAILURE;
+
+void msg_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+{
+  std::cout << "Message received" << std::endl;
+  msg_received = EXIT_SUCCESS;
+  rclcpp::shutdown();
+}
+
+void timeout_callback()
+{
+  std::cout << "Timeout" << std::endl;
+  rclcpp::shutdown();
+}
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("healthcheck_node");
+  rclcpp::QoS qos(1);
+  qos.transient_local();
+  auto sub = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(TOPIC_NAME, qos, msg_callback);
+  auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
+
+  rclcpp::spin(node);
+  return msg_received;
+}
+
diff --git a/healthcheck_localization.py b/healthcheck_localization.py
deleted file mode 100755
index ec66b53..0000000
--- a/healthcheck_localization.py
+++ /dev/null
@@ -1,21 +0,0 @@
-#!/usr/bin/python3
-import rclpy
-import os
-from geometry_msgs.msg import PoseWithCovarianceStamped
-from rclpy.qos import QoSProfile, QoSDurabilityPolicy
-
-print ("SLAM_MODE:" + os.environ.get("SLAM_MODE"))
-
-def callback(msg):
-    print(f"received amcl_pose")
-    exit(0)
-
-rclpy.init(args=[])
-latching_qos = QoSProfile(depth=1,
-            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
-
-node = rclpy.create_node("amcl_pose_listener")
-sub = node.create_subscription(PoseWithCovarianceStamped, "/amcl_pose", callback, qos_profile=latching_qos)
-rclpy.spin_once(node)
-print(f"amcl_pose timeout")
-exit(1)
diff --git a/healthcheck_navigation.cpp b/healthcheck_navigation.cpp
new file mode 100644
index 0000000..5715c0f
--- /dev/null
+++ b/healthcheck_navigation.cpp
@@ -0,0 +1,28 @@
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+
+    // Create a ROS node
+    auto node = std::make_shared<rclcpp::Node>("healthcheck_node");
+
+    // Get a list of active node names
+    auto node_names = node->get_node_names();
+
+    bool bt_navigator_started = false;
+    for (const auto &name : node_names) {
+        if (name == "/bt_navigator") {
+            bt_navigator_started = true;
+            break;
+        }
+    }
+
+    if (bt_navigator_started) {
+        std::cout << "Node '/bt_navigator' has started." << std::endl;
+    } else {
+        std::cout << "Node '/bt_navigator' has not started yet." << std::endl;
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}
diff --git a/healthcheck_slam.cpp b/healthcheck_slam.cpp
new file mode 100755
index 0000000..b2f0f12
--- /dev/null
+++ b/healthcheck_slam.cpp
@@ -0,0 +1,73 @@
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_msgs/srv/save_map.hpp"
+#include "cstdlib"
+
+using namespace std::chrono_literals;
+
+#define TIMEOUT 3s
+
+int msg_received = false;
+int map_saved = false;
+
+void msg_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
+{
+  std::cout << "Message received" << std::endl;
+  msg_received = true;
+  if (msg_received && map_saved)
+  {
+    rclcpp::shutdown();
+  }
+}
+
+void timeout_callback()
+{
+  std::cout << "Timeout" << std::endl;
+  rclcpp::shutdown();
+}
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("healthcheck_node");
+  auto sub = node->create_subscription<nav_msgs::msg::OccupancyGrid>("/map", rclcpp::SensorDataQoS(),
+                                                                msg_callback);
+  auto client = node->create_client<nav2_msgs::srv::SaveMap>("/map_saver/save_map");
+  auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
+
+  if (!client->wait_for_service(TIMEOUT))
+  {
+    std::cout << "Failed to connect to the image save service" << std::endl;
+    rclcpp::shutdown();
+  }
+
+  auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  request->free_thresh = 0.15;
+  request->map_topic = "/map";
+  request->map_url = "/maps/map";
+  request->map_mode = "trinary";
+  request->image_format = "png";
+
+  auto future = client->async_send_request(request);
+
+  if (rclcpp::spin_until_future_complete(node, future) == rclcpp::FutureReturnCode::SUCCESS)
+  {
+    std::cout << "Response received" << std::endl;
+    map_saved = true;
+    if (msg_received && map_saved)
+    {
+      rclcpp::shutdown();
+    }
+  }
+
+  rclcpp::spin(node);
+
+  if (msg_received && map_saved)
+  {
+    return EXIT_SUCCESS;
+  }
+  else
+  {
+    return EXIT_FAILURE;
+  }
+}
diff --git a/healthcheck_slam.py b/healthcheck_slam.py
deleted file mode 100755
index 967e698..0000000
--- a/healthcheck_slam.py
+++ /dev/null
@@ -1,26 +0,0 @@
-#!/usr/bin/python3
-import rclpy
-import os
-from nav2_msgs.srv import SaveMap
-
-print ("SLAM_MODE:" + os.environ.get("SLAM_MODE"))
-
-rclpy.init(args=[])
-node = rclpy.create_node("map_saver")
-client = node.create_client(SaveMap, '/map_saver/save_map')
-
-if not client.wait_for_service(timeout_sec=2.0):
-    print(f"map timeout")
-    exit (1)
-
-request = SaveMap.Request()
-request.free_thresh = 0.15
-request.map_topic = "/map"
-request.map_url = "/maps/map"
-request.map_mode = "trinary"
-request.image_format = "png"
-
-future = client.call_async(request)
-rclpy.spin_until_future_complete(node, future)
-print(f"map saved")
-exit(0)
\ No newline at end of file

From b0ea5deb0f48b374d5addb76abde4dcd4f005e73 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 9 Nov 2023 16:04:23 +0100
Subject: [PATCH 02/25] Update healthcheck_navigation.cpp

---
 healthcheck_navigation.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/healthcheck_navigation.cpp b/healthcheck_navigation.cpp
index 5715c0f..0016e68 100644
--- a/healthcheck_navigation.cpp
+++ b/healthcheck_navigation.cpp
@@ -1,3 +1,4 @@
+#include "cstdlib"
 #include "rclcpp/rclcpp.hpp"
 
 int main(int argc, char **argv) {
@@ -17,12 +18,13 @@ int main(int argc, char **argv) {
         }
     }
 
+    rclcpp::shutdown();
+
     if (bt_navigator_started) {
         std::cout << "Node '/bt_navigator' has started." << std::endl;
+        return EXIT_SUCCESS;
     } else {
         std::cout << "Node '/bt_navigator' has not started yet." << std::endl;
+        return EXIT_FAILURE;
     }
-
-    rclcpp::shutdown();
-    return 0;
 }

From 1d2c38e3a0ad07b174fa68b955d736e5de383836 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 9 Nov 2023 16:05:40 +0100
Subject: [PATCH 03/25] Update healthcheck_slam.cpp

---
 healthcheck_slam.cpp | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

diff --git a/healthcheck_slam.cpp b/healthcheck_slam.cpp
index b2f0f12..a24ee34 100755
--- a/healthcheck_slam.cpp
+++ b/healthcheck_slam.cpp
@@ -62,12 +62,5 @@ int main(int argc, char* argv[])
 
   rclcpp::spin(node);
 
-  if (msg_received && map_saved)
-  {
-    return EXIT_SUCCESS;
-  }
-  else
-  {
-    return EXIT_FAILURE;
-  }
+  return (msg_received && map_saved) ? EXIT_SUCCESS : EXIT_FAILURE;
 }

From 0cd9920f22dbbb9491b52f9614cfe4120630da88 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Thu, 9 Nov 2023 16:06:43 +0100
Subject: [PATCH 04/25] Update healthcheck_localization.cpp

---
 healthcheck_localization.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/healthcheck_localization.cpp b/healthcheck_localization.cpp
index 9128aaf..6fe6a13 100755
--- a/healthcheck_localization.cpp
+++ b/healthcheck_localization.cpp
@@ -4,7 +4,6 @@
 
 using namespace std::chrono_literals;
 
-#define TOPIC_NAME "/amcl_pose"
 #define TIMEOUT 2s
 
 int msg_received = EXIT_FAILURE;
@@ -28,7 +27,7 @@ int main(int argc, char* argv[])
   auto node = rclcpp::Node::make_shared("healthcheck_node");
   rclcpp::QoS qos(1);
   qos.transient_local();
-  auto sub = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(TOPIC_NAME, qos, msg_callback);
+  auto sub = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/amcl_pose", qos, msg_callback);
   auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
 
   rclcpp::spin(node);

From 796a6d373a93dc6fa6081a4f57d87271bb6f2493 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Mon, 13 Nov 2023 17:23:49 +0100
Subject: [PATCH 05/25] Timeout

---
 .gitignore | 1 +
 Dockerfile | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)
 create mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..722d5e7
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+.vscode
diff --git a/Dockerfile b/Dockerfile
index 3e481e6..3bf8c39 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -51,7 +51,7 @@ RUN source /opt/ros/$ROS_DISTRO/setup.bash && \
 
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
-HEALTHCHECK --interval=15s --timeout=5s --start-period=10s --retries=3 \
+HEALTHCHECK --interval=20s --timeout=10s --start-period=10s --retries=3 \
     CMD bash -c "/ros_entrypoint.sh ros2 run healthcheck_pkg healthcheck_$SLAM_MODE"
 
 #tip: gathering logs from healthcheck: docker inspect b39 | jq '.[0].State.Health.Log'

From 1e98e56e3edfdaa15cbc9ac32bb412b95c290b93 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 14 Nov 2023 09:55:45 +0100
Subject: [PATCH 06/25] Fix navigation healthcheck

---
 Dockerfile                 |  3 ++-
 healthcheck_navigation.cpp | 47 ++++++++++++++++++++------------------
 2 files changed, 27 insertions(+), 23 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 3bf8c39..1a5aa25 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -28,12 +28,13 @@ RUN mkdir src && cd src/ && \
     sed -i '/find_package(std_msgs REQUIRED)/a \
             find_package(geometry_msgs REQUIRED)\n \
             find_package(nav2_msgs REQUIRED)\n \
+            find_package(bond REQUIRED)\n \
             add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
             ament_target_dependencies(healthcheck_localization rclcpp std_msgs geometry_msgs)\n \
             add_executable(healthcheck_slam src/healthcheck_slam.cpp)\n \
             ament_target_dependencies(healthcheck_slam rclcpp std_msgs nav2_msgs)\n \
             add_executable(healthcheck_ src/healthcheck_navigation.cpp)\n \
-            ament_target_dependencies(healthcheck_ rclcpp std_msgs geometry_msgs)\n \
+            ament_target_dependencies(healthcheck_ rclcpp std_msgs bond)\n \
             install(TARGETS \
                 healthcheck_localization \
                 healthcheck_slam  \
diff --git a/healthcheck_navigation.cpp b/healthcheck_navigation.cpp
index 0016e68..2ad2b29 100644
--- a/healthcheck_navigation.cpp
+++ b/healthcheck_navigation.cpp
@@ -1,30 +1,33 @@
-#include "cstdlib"
 #include "rclcpp/rclcpp.hpp"
+#include "bond/msg/status.hpp"
+#include "cstdlib"
 
-int main(int argc, char **argv) {
-    rclcpp::init(argc, argv);
+using namespace std::chrono_literals;
 
-    // Create a ROS node
-    auto node = std::make_shared<rclcpp::Node>("healthcheck_node");
+#define TIMEOUT 2s
 
-    // Get a list of active node names
-    auto node_names = node->get_node_names();
+int msg_received = EXIT_FAILURE;
 
-    bool bt_navigator_started = false;
-    for (const auto &name : node_names) {
-        if (name == "/bt_navigator") {
-            bt_navigator_started = true;
-            break;
-        }
-    }
+void msg_callback(const bond::msg::Status::SharedPtr msg)
+{
+  std::cout << "Message received" << std::endl;
+  msg_received = EXIT_SUCCESS;
+  rclcpp::shutdown();
+}
+
+void timeout_callback()
+{
+  std::cout << "Timeout" << std::endl;
+  rclcpp::shutdown();
+}
 
-    rclcpp::shutdown();
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("healthcheck_node");
+  auto sub = node->create_subscription<bond::msg::Status>("/bond", rclcpp::SystemDefaultsQoS(), msg_callback);
+  auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
 
-    if (bt_navigator_started) {
-        std::cout << "Node '/bt_navigator' has started." << std::endl;
-        return EXIT_SUCCESS;
-    } else {
-        std::cout << "Node '/bt_navigator' has not started yet." << std::endl;
-        return EXIT_FAILURE;
-    }
+  rclcpp::spin(node);
+  return msg_received;
 }

From 192c5bc45687cfa3470519c8cc5102d2f7fba556 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 12:17:00 +0100
Subject: [PATCH 07/25]  New version of Healthcheck

---
 Dockerfile                                    |  15 ++-
 healthcheck/healthcheck.sh                    |  28 +++++
 .../healthcheck_localization.cpp              |  34 ++---
 healthcheck/healthcheck_navigation.cpp        |  57 +++++++++
 healthcheck/healthcheck_slam.cpp              | 116 ++++++++++++++++++
 healthcheck_navigation.cpp                    |  33 -----
 healthcheck_slam.cpp                          |  66 ----------
 7 files changed, 228 insertions(+), 121 deletions(-)
 create mode 100755 healthcheck/healthcheck.sh
 rename healthcheck_localization.cpp => healthcheck/healthcheck_localization.cpp (51%)
 create mode 100644 healthcheck/healthcheck_navigation.cpp
 create mode 100755 healthcheck/healthcheck_slam.cpp
 delete mode 100644 healthcheck_navigation.cpp
 delete mode 100755 healthcheck_slam.cpp

diff --git a/Dockerfile b/Dockerfile
index 1a5aa25..188d0fb 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -30,11 +30,11 @@ RUN mkdir src && cd src/ && \
             find_package(nav2_msgs REQUIRED)\n \
             find_package(bond REQUIRED)\n \
             add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
-            ament_target_dependencies(healthcheck_localization rclcpp std_msgs geometry_msgs)\n \
+            ament_target_dependencies(healthcheck_localization rclcpp geometry_msgs)\n \
             add_executable(healthcheck_slam src/healthcheck_slam.cpp)\n \
-            ament_target_dependencies(healthcheck_slam rclcpp std_msgs nav2_msgs)\n \
+            ament_target_dependencies(healthcheck_slam rclcpp nav2_msgs)\n \
             add_executable(healthcheck_ src/healthcheck_navigation.cpp)\n \
-            ament_target_dependencies(healthcheck_ rclcpp std_msgs bond)\n \
+            ament_target_dependencies(healthcheck_ rclcpp bond)\n \
             install(TARGETS \
                 healthcheck_localization \
                 healthcheck_slam  \
@@ -42,9 +42,7 @@ RUN mkdir src && cd src/ && \
                 DESTINATION lib/${PROJECT_NAME})' \
             /ros2_ws/src/healthcheck_pkg/CMakeLists.txt
 
-COPY ./healthcheck_localization.cpp /ros2_ws/src/healthcheck_pkg/src/
-COPY ./healthcheck_slam.cpp /ros2_ws/src/healthcheck_pkg/src/
-COPY ./healthcheck_navigation.cpp /ros2_ws/src/healthcheck_pkg/src/
+COPY ./healthcheck /ros2_ws/src/healthcheck_pkg/src
 
 # Build
 RUN source /opt/ros/$ROS_DISTRO/setup.bash && \
@@ -52,7 +50,8 @@ RUN source /opt/ros/$ROS_DISTRO/setup.bash && \
 
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
-HEALTHCHECK --interval=20s --timeout=10s --start-period=10s --retries=3 \
-    CMD bash -c "/ros_entrypoint.sh ros2 run healthcheck_pkg healthcheck_$SLAM_MODE"
+COPY ./healthcheck/healthcheck.sh /
+HEALTHCHECK --interval=10s --timeout=2s --start-period=5s --retries=5 \
+    CMD ["/healthcheck.sh"]
 
 #tip: gathering logs from healthcheck: docker inspect b39 | jq '.[0].State.Health.Log'
diff --git a/healthcheck/healthcheck.sh b/healthcheck/healthcheck.sh
new file mode 100755
index 0000000..dee593c
--- /dev/null
+++ b/healthcheck/healthcheck.sh
@@ -0,0 +1,28 @@
+#!/bin/bash
+
+HEALTHCHECK_FILE="/health_status.txt"
+
+# Function to start the ROS 2 healthcheck node
+start_healthcheck_node() {
+    /ros_entrypoint.sh ros2 run healthcheck_pkg healthcheck_$SLAM_MODE &
+}
+
+if [ ! -f "$HEALTHCHECK_FILE" ]; then
+    echo "Healthcheck file not found. Starting ROS 2 healthcheck node..."
+    start_healthcheck_node
+    # Wait a bit to allow the node to start and write its initial status
+    sleep 2
+fi
+
+# Now check the health status
+if [ -f "$HEALTHCHECK_FILE" ]; then
+    status=$(cat "$HEALTHCHECK_FILE")
+    if [ "$status" == "healthy" ]; then
+        exit 0
+    else
+        exit 1
+    fi
+else
+    echo "Healthcheck file still not found. There may be an issue with the node."
+    exit 1
+fi
diff --git a/healthcheck_localization.cpp b/healthcheck/healthcheck_localization.cpp
similarity index 51%
rename from healthcheck_localization.cpp
rename to healthcheck/healthcheck_localization.cpp
index 6fe6a13..f433794 100755
--- a/healthcheck_localization.cpp
+++ b/healthcheck/healthcheck_localization.cpp
@@ -1,24 +1,26 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "cstdlib"
+#include "fstream"
 
 using namespace std::chrono_literals;
 
-#define TIMEOUT 2s
+#define LOOP_PERIOD 2s
 
-int msg_received = EXIT_FAILURE;
+// State 'healthy' is latched, because topic 'amcl_pose' is transient_local and send only when moving.
 
-void msg_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+std::chrono::steady_clock::time_point last_msg_time;
+
+void write_health_status(const std::string& status)
 {
-  std::cout << "Message received" << std::endl;
-  msg_received = EXIT_SUCCESS;
-  rclcpp::shutdown();
+  std::ofstream healthFile("/health_status.txt");
+  healthFile << status;
+  healthFile.close();
 }
 
-void timeout_callback()
+void msg_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
 {
-  std::cout << "Timeout" << std::endl;
-  rclcpp::shutdown();
+  std::cout << "Message received" << std::endl;
+  write_health_status("healthy");
 }
 
 int main(int argc, char* argv[])
@@ -28,9 +30,13 @@ int main(int argc, char* argv[])
   rclcpp::QoS qos(1);
   qos.transient_local();
   auto sub = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/amcl_pose", qos, msg_callback);
-  auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
 
-  rclcpp::spin(node);
-  return msg_received;
-}
+  write_health_status("unhealthy");
+  while (rclcpp::ok())
+  {
+    rclcpp::spin_some(node);
+    std::this_thread::sleep_for(LOOP_PERIOD);
+  }
 
+  return 0;
+}
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
new file mode 100644
index 0000000..484c5e8
--- /dev/null
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -0,0 +1,57 @@
+#include "rclcpp/rclcpp.hpp"
+#include "bond/msg/status.hpp"
+#include "fstream"
+
+using namespace std::chrono_literals;
+
+#define LOOP_PERIOD 2s
+#define MSG_VALID_TIME 5s
+
+std::chrono::steady_clock::time_point last_msg_time;
+
+void write_health_status(const std::string& status)
+{
+  std::ofstream healthFile("/health_status.txt");
+  healthFile << status;
+  healthFile.close();
+}
+
+void msg_callback(const bond::msg::Status::SharedPtr msg)
+{
+  write_health_status("healthy");
+  last_msg_time = std::chrono::steady_clock::now();
+}
+
+void healthy_check(const rclcpp::Node::SharedPtr& node)
+{
+  std::chrono::steady_clock::time_point current_time = std::chrono::steady_clock::now();
+  std::chrono::duration<double> elapsed_time = current_time - last_msg_time;
+  bool is_msg_valid = elapsed_time.count() < MSG_VALID_TIME.count();
+
+  if (is_msg_valid)
+  {
+    RCLCPP_INFO(node->get_logger(), "Health check: healthy");
+    write_health_status("healthy");
+  }
+  else
+  {
+    RCLCPP_WARN(node->get_logger(), "Health check: unhealthy");
+    write_health_status("unhealthy");
+  }
+}
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("healthcheck_node");
+  auto sub = node->create_subscription<bond::msg::Status>("/bond", rclcpp::SystemDefaultsQoS(), msg_callback);
+
+  while (rclcpp::ok())
+  {
+    rclcpp::spin_some(node);
+    healthy_check(node);
+    std::this_thread::sleep_for(LOOP_PERIOD);
+  }
+
+  return 0;
+}
diff --git a/healthcheck/healthcheck_slam.cpp b/healthcheck/healthcheck_slam.cpp
new file mode 100755
index 0000000..c6c1ce4
--- /dev/null
+++ b/healthcheck/healthcheck_slam.cpp
@@ -0,0 +1,116 @@
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_msgs/srv/save_map.hpp"
+#include <fstream>  // Include <fstream> for file operations
+#include <thread>   // Include <thread> for sleep_for
+
+using namespace std::chrono_literals;
+
+#define LOOP_PERIOD 2s
+#define MSG_VALID_TIME 5s
+#define SAVE_MAP_PERIOD 15s
+#define SAVE_MAP_CONNECTION_TIMEOUT 2s
+#define SAVE_MAP_VALID_TIME 20s
+
+class HealthCheckNode : public rclcpp::Node
+{
+public:
+  HealthCheckNode() : Node("healthcheck_node")
+  {
+    rclcpp::QoS qos(1);
+    qos.transient_local();
+    map_subscriber_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+        "/map", qos, std::bind(&HealthCheckNode::msgCallback, this, std::placeholders::_1));
+    save_map_client_ = create_client<nav2_msgs::srv::SaveMap>("/map_saver/save_map");
+  }
+
+  void healthyCheck()
+  {
+    auto current_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> msg_elapsed_time = current_time - last_msg_time;
+    std::chrono::duration<double> saved_map_elapsed_time = current_time - last_saved_map_time;
+    bool is_msg_valid = msg_elapsed_time.count() < MSG_VALID_TIME.count();
+    bool is_save_map_valid = saved_map_elapsed_time.count() < SAVE_MAP_VALID_TIME.count();
+
+    if (is_msg_valid && is_save_map_valid)
+    {
+      RCLCPP_INFO(get_logger(), "Health check: healthy");
+      writeHealthStatus("healthy");
+    }
+    else
+    {
+      RCLCPP_WARN(get_logger(), "Health check: unhealthy");
+      writeHealthStatus("unhealthy");
+    }
+  }
+
+  void saveMapPeriodically()
+  {
+    auto current_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed_time = current_time - last_saved_map_time;
+
+    if (elapsed_time.count() > SAVE_MAP_PERIOD.count())
+    {
+      if (save_map_client_->wait_for_service(SAVE_MAP_CONNECTION_TIMEOUT))
+      {
+        RCLCPP_DEBUG(get_logger(), "Service available");
+        auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+        request->free_thresh = 0.15;
+        request->map_topic = "/map";
+        request->map_url = "/maps/map";
+        request->map_mode = "trinary";
+        request->image_format = "png";
+
+        auto future = save_map_client_->async_send_request(request);
+
+        if (rclcpp::spin_until_future_complete(shared_from_this(), future, SAVE_MAP_CONNECTION_TIMEOUT) == rclcpp::FutureReturnCode::SUCCESS)
+        {
+          RCLCPP_DEBUG(get_logger(), "Map saved");
+          last_saved_map_time = std::chrono::steady_clock::now();
+        }
+        else
+        {
+          RCLCPP_DEBUG(get_logger(), "Service response didn't arrived");
+        }
+      }
+      else
+      {
+        RCLCPP_DEBUG(get_logger(), "Service unavailable");
+      }
+    }
+  }
+
+private:
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_subscriber_;
+  rclcpp::Client<nav2_msgs::srv::SaveMap>::SharedPtr save_map_client_;
+  std::chrono::steady_clock::time_point last_msg_time;
+  std::chrono::steady_clock::time_point last_saved_map_time;
+
+  void writeHealthStatus(const std::string &status)
+  {
+    std::ofstream healthFile("/health_status.txt");
+    healthFile << status;
+  }
+
+  void msgCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
+  {
+    RCLCPP_DEBUG(get_logger(), "Msg arrived");
+    last_msg_time = std::chrono::steady_clock::now();
+  }
+};
+
+int main(int argc, char *argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<HealthCheckNode>();
+
+  while (rclcpp::ok())
+  {
+    rclcpp::spin_some(node);
+    node->healthyCheck();
+    node->saveMapPeriodically();
+    std::this_thread::sleep_for(LOOP_PERIOD);
+  }
+
+  return 0;
+}
diff --git a/healthcheck_navigation.cpp b/healthcheck_navigation.cpp
deleted file mode 100644
index 2ad2b29..0000000
--- a/healthcheck_navigation.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-#include "bond/msg/status.hpp"
-#include "cstdlib"
-
-using namespace std::chrono_literals;
-
-#define TIMEOUT 2s
-
-int msg_received = EXIT_FAILURE;
-
-void msg_callback(const bond::msg::Status::SharedPtr msg)
-{
-  std::cout << "Message received" << std::endl;
-  msg_received = EXIT_SUCCESS;
-  rclcpp::shutdown();
-}
-
-void timeout_callback()
-{
-  std::cout << "Timeout" << std::endl;
-  rclcpp::shutdown();
-}
-
-int main(int argc, char* argv[])
-{
-  rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("healthcheck_node");
-  auto sub = node->create_subscription<bond::msg::Status>("/bond", rclcpp::SystemDefaultsQoS(), msg_callback);
-  auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
-
-  rclcpp::spin(node);
-  return msg_received;
-}
diff --git a/healthcheck_slam.cpp b/healthcheck_slam.cpp
deleted file mode 100755
index a24ee34..0000000
--- a/healthcheck_slam.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-#include "nav_msgs/msg/occupancy_grid.hpp"
-#include "nav2_msgs/srv/save_map.hpp"
-#include "cstdlib"
-
-using namespace std::chrono_literals;
-
-#define TIMEOUT 3s
-
-int msg_received = false;
-int map_saved = false;
-
-void msg_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
-{
-  std::cout << "Message received" << std::endl;
-  msg_received = true;
-  if (msg_received && map_saved)
-  {
-    rclcpp::shutdown();
-  }
-}
-
-void timeout_callback()
-{
-  std::cout << "Timeout" << std::endl;
-  rclcpp::shutdown();
-}
-
-int main(int argc, char* argv[])
-{
-  rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("healthcheck_node");
-  auto sub = node->create_subscription<nav_msgs::msg::OccupancyGrid>("/map", rclcpp::SensorDataQoS(),
-                                                                msg_callback);
-  auto client = node->create_client<nav2_msgs::srv::SaveMap>("/map_saver/save_map");
-  auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
-
-  if (!client->wait_for_service(TIMEOUT))
-  {
-    std::cout << "Failed to connect to the image save service" << std::endl;
-    rclcpp::shutdown();
-  }
-
-  auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
-  request->free_thresh = 0.15;
-  request->map_topic = "/map";
-  request->map_url = "/maps/map";
-  request->map_mode = "trinary";
-  request->image_format = "png";
-
-  auto future = client->async_send_request(request);
-
-  if (rclcpp::spin_until_future_complete(node, future) == rclcpp::FutureReturnCode::SUCCESS)
-  {
-    std::cout << "Response received" << std::endl;
-    map_saved = true;
-    if (msg_received && map_saved)
-    {
-      rclcpp::shutdown();
-    }
-  }
-
-  rclcpp::spin(node);
-
-  return (msg_received && map_saved) ? EXIT_SUCCESS : EXIT_FAILURE;
-}

From 7eadb4a9840d3cae163a0a48aa3c864a2bbff472 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 12:26:43 +0100
Subject: [PATCH 08/25] delete unnecessary

---
 healthcheck/healthcheck_localization.cpp | 1 -
 healthcheck/healthcheck_navigation.cpp   | 1 -
 2 files changed, 2 deletions(-)

diff --git a/healthcheck/healthcheck_localization.cpp b/healthcheck/healthcheck_localization.cpp
index f433794..cd2b769 100755
--- a/healthcheck/healthcheck_localization.cpp
+++ b/healthcheck/healthcheck_localization.cpp
@@ -14,7 +14,6 @@ void write_health_status(const std::string& status)
 {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
-  healthFile.close();
 }
 
 void msg_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
index 484c5e8..4c5062b 100644
--- a/healthcheck/healthcheck_navigation.cpp
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -13,7 +13,6 @@ void write_health_status(const std::string& status)
 {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
-  healthFile.close();
 }
 
 void msg_callback(const bond::msg::Status::SharedPtr msg)

From cd6914abed70f384b60e2db3a68bf3eb3836f441 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 13:38:17 +0100
Subject: [PATCH 09/25] little changes

---
 healthcheck/healthcheck_localization.cpp | 2 --
 healthcheck/healthcheck_navigation.cpp   | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/healthcheck/healthcheck_localization.cpp b/healthcheck/healthcheck_localization.cpp
index cd2b769..4de4159 100755
--- a/healthcheck/healthcheck_localization.cpp
+++ b/healthcheck/healthcheck_localization.cpp
@@ -8,8 +8,6 @@ using namespace std::chrono_literals;
 
 // State 'healthy' is latched, because topic 'amcl_pose' is transient_local and send only when moving.
 
-std::chrono::steady_clock::time_point last_msg_time;
-
 void write_health_status(const std::string& status)
 {
   std::ofstream healthFile("/health_status.txt");
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
index 4c5062b..18e45b6 100644
--- a/healthcheck/healthcheck_navigation.cpp
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -17,7 +17,7 @@ void write_health_status(const std::string& status)
 
 void msg_callback(const bond::msg::Status::SharedPtr msg)
 {
-  write_health_status("healthy");
+  std::cout<<"Message received"<<std::endl;
   last_msg_time = std::chrono::steady_clock::now();
 }
 

From 9b822d0b10dc333e1ac23d4c3213b86357a8f9dd Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 13:41:27 +0100
Subject: [PATCH 10/25] docker update

---
 Dockerfile | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 188d0fb..3294d64 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -6,7 +6,7 @@ FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base
 SHELL ["/bin/bash", "-c"]
 
 ARG PREFIX
-ENV SLAM_MODE=
+ENV SLAM_MODE=navigation
 ENV PREFIX_ENV=$PREFIX
 
 RUN apt update && apt upgrade -y && apt install -y \
@@ -31,14 +31,14 @@ RUN mkdir src && cd src/ && \
             find_package(bond REQUIRED)\n \
             add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
             ament_target_dependencies(healthcheck_localization rclcpp geometry_msgs)\n \
+            add_executable(healthcheck_navigation src/healthcheck_navigation.cpp)\n \
+            ament_target_dependencies(healthcheck_navigation rclcpp bond)\n \
             add_executable(healthcheck_slam src/healthcheck_slam.cpp)\n \
             ament_target_dependencies(healthcheck_slam rclcpp nav2_msgs)\n \
-            add_executable(healthcheck_ src/healthcheck_navigation.cpp)\n \
-            ament_target_dependencies(healthcheck_ rclcpp bond)\n \
             install(TARGETS \
                 healthcheck_localization \
+                healthcheck_navigation \
                 healthcheck_slam  \
-                healthcheck_ \
                 DESTINATION lib/${PROJECT_NAME})' \
             /ros2_ws/src/healthcheck_pkg/CMakeLists.txt
 

From 8d741551d32a5cf93ea5b564f75d841cd6c4cd60 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 15:27:13 +0100
Subject: [PATCH 11/25] formatting

---
 healthcheck/healthcheck_localization.cpp | 24 ++++----
 healthcheck/healthcheck_navigation.cpp   | 32 ++++------
 healthcheck/healthcheck_slam.cpp         | 76 ++++++++++--------------
 3 files changed, 58 insertions(+), 74 deletions(-)

diff --git a/healthcheck/healthcheck_localization.cpp b/healthcheck/healthcheck_localization.cpp
index 4de4159..6f47a44 100755
--- a/healthcheck/healthcheck_localization.cpp
+++ b/healthcheck/healthcheck_localization.cpp
@@ -1,36 +1,36 @@
-#include "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "fstream"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
 
 #define LOOP_PERIOD 2s
 
-// State 'healthy' is latched, because topic 'amcl_pose' is transient_local and send only when moving.
+// State 'healthy' is latched, because topic 'amcl_pose' is transient_local and
+// send only when moving.
 
-void write_health_status(const std::string& status)
-{
+void write_health_status(const std::string &status) {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
 }
 
-void msg_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
-{
+void msg_callback(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
   std::cout << "Message received" << std::endl;
   write_health_status("healthy");
 }
 
-int main(int argc, char* argv[])
-{
+int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("healthcheck_node");
   rclcpp::QoS qos(1);
   qos.transient_local();
-  auto sub = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/amcl_pose", qos, msg_callback);
+  auto sub =
+      node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+          "/amcl_pose", qos, msg_callback);
 
   write_health_status("unhealthy");
-  while (rclcpp::ok())
-  {
+  while (rclcpp::ok()) {
     rclcpp::spin_some(node);
     std::this_thread::sleep_for(LOOP_PERIOD);
   }
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
index 18e45b6..e972d74 100644
--- a/healthcheck/healthcheck_navigation.cpp
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -1,6 +1,6 @@
-#include "rclcpp/rclcpp.hpp"
 #include "bond/msg/status.hpp"
 #include "fstream"
+#include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
 
@@ -9,44 +9,38 @@ using namespace std::chrono_literals;
 
 std::chrono::steady_clock::time_point last_msg_time;
 
-void write_health_status(const std::string& status)
-{
+void write_health_status(const std::string &status) {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
 }
 
-void msg_callback(const bond::msg::Status::SharedPtr msg)
-{
-  std::cout<<"Message received"<<std::endl;
+void msg_callback(const bond::msg::Status::SharedPtr msg) {
+  std::cout << "Message received" << std::endl;
   last_msg_time = std::chrono::steady_clock::now();
 }
 
-void healthy_check(const rclcpp::Node::SharedPtr& node)
-{
-  std::chrono::steady_clock::time_point current_time = std::chrono::steady_clock::now();
+void healthy_check(const rclcpp::Node::SharedPtr &node) {
+  std::chrono::steady_clock::time_point current_time =
+      std::chrono::steady_clock::now();
   std::chrono::duration<double> elapsed_time = current_time - last_msg_time;
   bool is_msg_valid = elapsed_time.count() < MSG_VALID_TIME.count();
 
-  if (is_msg_valid)
-  {
+  if (is_msg_valid) {
     RCLCPP_INFO(node->get_logger(), "Health check: healthy");
     write_health_status("healthy");
-  }
-  else
-  {
+  } else {
     RCLCPP_WARN(node->get_logger(), "Health check: unhealthy");
     write_health_status("unhealthy");
   }
 }
 
-int main(int argc, char* argv[])
-{
+int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("healthcheck_node");
-  auto sub = node->create_subscription<bond::msg::Status>("/bond", rclcpp::SystemDefaultsQoS(), msg_callback);
+  auto sub = node->create_subscription<bond::msg::Status>(
+      "/bond", rclcpp::SystemDefaultsQoS(), msg_callback);
 
-  while (rclcpp::ok())
-  {
+  while (rclcpp::ok()) {
     rclcpp::spin_some(node);
     healthy_check(node);
     std::this_thread::sleep_for(LOOP_PERIOD);
diff --git a/healthcheck/healthcheck_slam.cpp b/healthcheck/healthcheck_slam.cpp
index c6c1ce4..5695a88 100755
--- a/healthcheck/healthcheck_slam.cpp
+++ b/healthcheck/healthcheck_slam.cpp
@@ -1,8 +1,8 @@
-#include "rclcpp/rclcpp.hpp"
-#include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav2_msgs/srv/save_map.hpp"
-#include <fstream>  // Include <fstream> for file operations
-#include <thread>   // Include <thread> for sleep_for
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <fstream>
+#include <thread>
 
 using namespace std::chrono_literals;
 
@@ -12,47 +12,44 @@ using namespace std::chrono_literals;
 #define SAVE_MAP_CONNECTION_TIMEOUT 2s
 #define SAVE_MAP_VALID_TIME 20s
 
-class HealthCheckNode : public rclcpp::Node
-{
+class HealthCheckNode : public rclcpp::Node {
 public:
-  HealthCheckNode() : Node("healthcheck_node")
-  {
+  HealthCheckNode() : Node("healthcheck_node") {
     rclcpp::QoS qos(1);
     qos.transient_local();
     map_subscriber_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
-        "/map", qos, std::bind(&HealthCheckNode::msgCallback, this, std::placeholders::_1));
-    save_map_client_ = create_client<nav2_msgs::srv::SaveMap>("/map_saver/save_map");
+        "/map", qos,
+        std::bind(&HealthCheckNode::msgCallback, this, std::placeholders::_1));
+    save_map_client_ =
+        create_client<nav2_msgs::srv::SaveMap>("/map_saver/save_map");
   }
 
-  void healthyCheck()
-  {
+  void healthyCheck() {
     auto current_time = std::chrono::steady_clock::now();
-    std::chrono::duration<double> msg_elapsed_time = current_time - last_msg_time;
-    std::chrono::duration<double> saved_map_elapsed_time = current_time - last_saved_map_time;
+    std::chrono::duration<double> msg_elapsed_time =
+        current_time - last_msg_time;
+    std::chrono::duration<double> saved_map_elapsed_time =
+        current_time - last_saved_map_time;
     bool is_msg_valid = msg_elapsed_time.count() < MSG_VALID_TIME.count();
-    bool is_save_map_valid = saved_map_elapsed_time.count() < SAVE_MAP_VALID_TIME.count();
+    bool is_save_map_valid =
+        saved_map_elapsed_time.count() < SAVE_MAP_VALID_TIME.count();
 
-    if (is_msg_valid && is_save_map_valid)
-    {
+    if (is_msg_valid && is_save_map_valid) {
       RCLCPP_INFO(get_logger(), "Health check: healthy");
       writeHealthStatus("healthy");
-    }
-    else
-    {
+    } else {
       RCLCPP_WARN(get_logger(), "Health check: unhealthy");
       writeHealthStatus("unhealthy");
     }
   }
 
-  void saveMapPeriodically()
-  {
+  void saveMapPeriodically() {
     auto current_time = std::chrono::steady_clock::now();
-    std::chrono::duration<double> elapsed_time = current_time - last_saved_map_time;
+    std::chrono::duration<double> elapsed_time =
+        current_time - last_saved_map_time;
 
-    if (elapsed_time.count() > SAVE_MAP_PERIOD.count())
-    {
-      if (save_map_client_->wait_for_service(SAVE_MAP_CONNECTION_TIMEOUT))
-      {
+    if (elapsed_time.count() > SAVE_MAP_PERIOD.count()) {
+      if (save_map_client_->wait_for_service(SAVE_MAP_CONNECTION_TIMEOUT)) {
         RCLCPP_DEBUG(get_logger(), "Service available");
         auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
         request->free_thresh = 0.15;
@@ -63,18 +60,15 @@ class HealthCheckNode : public rclcpp::Node
 
         auto future = save_map_client_->async_send_request(request);
 
-        if (rclcpp::spin_until_future_complete(shared_from_this(), future, SAVE_MAP_CONNECTION_TIMEOUT) == rclcpp::FutureReturnCode::SUCCESS)
-        {
+        if (rclcpp::spin_until_future_complete(shared_from_this(), future,
+                                               SAVE_MAP_CONNECTION_TIMEOUT) ==
+            rclcpp::FutureReturnCode::SUCCESS) {
           RCLCPP_DEBUG(get_logger(), "Map saved");
           last_saved_map_time = std::chrono::steady_clock::now();
-        }
-        else
-        {
+        } else {
           RCLCPP_DEBUG(get_logger(), "Service response didn't arrived");
         }
-      }
-      else
-      {
+      } else {
         RCLCPP_DEBUG(get_logger(), "Service unavailable");
       }
     }
@@ -86,26 +80,22 @@ class HealthCheckNode : public rclcpp::Node
   std::chrono::steady_clock::time_point last_msg_time;
   std::chrono::steady_clock::time_point last_saved_map_time;
 
-  void writeHealthStatus(const std::string &status)
-  {
+  void writeHealthStatus(const std::string &status) {
     std::ofstream healthFile("/health_status.txt");
     healthFile << status;
   }
 
-  void msgCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
-  {
+  void msgCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
     RCLCPP_DEBUG(get_logger(), "Msg arrived");
     last_msg_time = std::chrono::steady_clock::now();
   }
 };
 
-int main(int argc, char *argv[])
-{
+int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = std::make_shared<HealthCheckNode>();
 
-  while (rclcpp::ok())
-  {
+  while (rclcpp::ok()) {
     rclcpp::spin_some(node);
     node->healthyCheck();
     node->saveMapPeriodically();

From 5a9db00a15a6f4b09dcd1625c95177acc495fa10 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 18:00:20 +0100
Subject: [PATCH 12/25] fix healhcheck

---
 .gitignore                               |  1 -
 Dockerfile                               | 44 ++++++++++++++----------
 healthcheck/healthcheck_localization.cpp | 31 ++++++++---------
 healthcheck/healthcheck_navigation.cpp   | 42 ++++++++--------------
 4 files changed, 56 insertions(+), 62 deletions(-)
 delete mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index 722d5e7..0000000
--- a/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-.vscode
diff --git a/Dockerfile b/Dockerfile
index 3294d64..eb0f606 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -1,23 +1,11 @@
 ARG ROS_DISTRO=humble
 ARG PREFIX=
 
-FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base
-
-SHELL ["/bin/bash", "-c"]
-
-ARG PREFIX
-ENV SLAM_MODE=navigation
-ENV PREFIX_ENV=$PREFIX
+### BUILD IMAGE ###
+FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS build
 
 RUN apt update && apt upgrade -y && apt install -y \
-        ros-$ROS_DISTRO-navigation2 \
-        ros-$ROS_DISTRO-nav2-bringup \
-        ros-$ROS_DISTRO-pointcloud-to-laserscan && \
-    apt-get autoremove -y && \
-    apt-get clean && \
-    rm -rf /var/lib/apt/lists/*
-
-COPY ./nav2_params /nav2_params
+        ros-$ROS_DISTRO-nav2-msgs \
 
 # Create healthcheck package
 WORKDIR /ros2_ws
@@ -28,11 +16,10 @@ RUN mkdir src && cd src/ && \
     sed -i '/find_package(std_msgs REQUIRED)/a \
             find_package(geometry_msgs REQUIRED)\n \
             find_package(nav2_msgs REQUIRED)\n \
-            find_package(bond REQUIRED)\n \
             add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
             ament_target_dependencies(healthcheck_localization rclcpp geometry_msgs)\n \
             add_executable(healthcheck_navigation src/healthcheck_navigation.cpp)\n \
-            ament_target_dependencies(healthcheck_navigation rclcpp bond)\n \
+            ament_target_dependencies(healthcheck_navigation rclcpp)\n \
             add_executable(healthcheck_slam src/healthcheck_slam.cpp)\n \
             ament_target_dependencies(healthcheck_slam rclcpp nav2_msgs)\n \
             install(TARGETS \
@@ -48,10 +35,31 @@ COPY ./healthcheck /ros2_ws/src/healthcheck_pkg/src
 RUN source /opt/ros/$ROS_DISTRO/setup.bash && \
     colcon build --event-handlers  console_direct+  --cmake-args  -DCMAKE_BUILD_TYPE=Release
 
+### RUNTIME IMAGE ###
+FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core
+
+ARG PREFIX
+ENV SLAM_MODE=navigation
+ENV PREFIX_ENV=$PREFIX
+
+WORKDIR /ros2_ws
+
+RUN apt update && apt upgrade -y && apt install -y \
+        ros-$ROS_DISTRO-navigation2 \
+        ros-$ROS_DISTRO-nav2-bringup \
+        ros-$ROS_DISTRO-pointcloud-to-laserscan && \
+    mkdir maps && \
+    apt-get autoremove -y && \
+    apt-get clean && \
+    rm -rf /var/lib/apt/lists/*
+
+COPY ./nav2_params /nav2_params
+COPY --from=build /ros2_ws /ros2_ws
+
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
 COPY ./healthcheck/healthcheck.sh /
-HEALTHCHECK --interval=10s --timeout=2s --start-period=5s --retries=5 \
+HEALTHCHECK --interval=10s --timeout=3s --start-period=5s --retries=5 \
     CMD ["/healthcheck.sh"]
 
 #tip: gathering logs from healthcheck: docker inspect b39 | jq '.[0].State.Health.Log'
diff --git a/healthcheck/healthcheck_localization.cpp b/healthcheck/healthcheck_localization.cpp
index 6f47a44..d43b9f7 100755
--- a/healthcheck/healthcheck_localization.cpp
+++ b/healthcheck/healthcheck_localization.cpp
@@ -1,37 +1,36 @@
 #include "fstream"
-#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
 
 #define LOOP_PERIOD 2s
 
-// State 'healthy' is latched, because topic 'amcl_pose' is transient_local and
-// send only when moving.
-
 void write_health_status(const std::string &status) {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
 }
 
-void msg_callback(
-    const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
-  std::cout << "Message received" << std::endl;
-  write_health_status("healthy");
-}
-
 int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("healthcheck_node");
-  rclcpp::QoS qos(1);
-  qos.transient_local();
-  auto sub =
-      node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-          "/amcl_pose", qos, msg_callback);
+  std::string node_to_find = "/amcl";
 
-  write_health_status("unhealthy");
   while (rclcpp::ok()) {
     rclcpp::spin_some(node);
+    auto node_names = node->get_node_names();
+    auto is_node_running = std::find(node_names.begin(), node_names.end(),
+                                     node_to_find) != node_names.end();
+
+    if (is_node_running) {
+      RCLCPP_INFO(node->get_logger(), "The %s node is running.",
+                  node_to_find.c_str());
+      write_health_status("healthy");
+
+    } else {
+      RCLCPP_WARN(node->get_logger(), "The %s node is not running.",
+                  node_to_find.c_str());
+      write_health_status("unhealthy");
+    }
     std::this_thread::sleep_for(LOOP_PERIOD);
   }
 
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
index e972d74..d43b9f7 100644
--- a/healthcheck/healthcheck_navigation.cpp
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -1,48 +1,36 @@
-#include "bond/msg/status.hpp"
 #include "fstream"
 #include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
 
 #define LOOP_PERIOD 2s
-#define MSG_VALID_TIME 5s
-
-std::chrono::steady_clock::time_point last_msg_time;
 
 void write_health_status(const std::string &status) {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
 }
 
-void msg_callback(const bond::msg::Status::SharedPtr msg) {
-  std::cout << "Message received" << std::endl;
-  last_msg_time = std::chrono::steady_clock::now();
-}
-
-void healthy_check(const rclcpp::Node::SharedPtr &node) {
-  std::chrono::steady_clock::time_point current_time =
-      std::chrono::steady_clock::now();
-  std::chrono::duration<double> elapsed_time = current_time - last_msg_time;
-  bool is_msg_valid = elapsed_time.count() < MSG_VALID_TIME.count();
-
-  if (is_msg_valid) {
-    RCLCPP_INFO(node->get_logger(), "Health check: healthy");
-    write_health_status("healthy");
-  } else {
-    RCLCPP_WARN(node->get_logger(), "Health check: unhealthy");
-    write_health_status("unhealthy");
-  }
-}
-
 int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("healthcheck_node");
-  auto sub = node->create_subscription<bond::msg::Status>(
-      "/bond", rclcpp::SystemDefaultsQoS(), msg_callback);
+  std::string node_to_find = "/amcl";
 
   while (rclcpp::ok()) {
     rclcpp::spin_some(node);
-    healthy_check(node);
+    auto node_names = node->get_node_names();
+    auto is_node_running = std::find(node_names.begin(), node_names.end(),
+                                     node_to_find) != node_names.end();
+
+    if (is_node_running) {
+      RCLCPP_INFO(node->get_logger(), "The %s node is running.",
+                  node_to_find.c_str());
+      write_health_status("healthy");
+
+    } else {
+      RCLCPP_WARN(node->get_logger(), "The %s node is not running.",
+                  node_to_find.c_str());
+      write_health_status("unhealthy");
+    }
     std::this_thread::sleep_for(LOOP_PERIOD);
   }
 

From 222fe52f2d154b15ea4707077656827094a5f783 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 18:26:52 +0100
Subject: [PATCH 13/25] typo

---
 Dockerfile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Dockerfile b/Dockerfile
index eb0f606..9e22bb5 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -5,7 +5,7 @@ ARG PREFIX=
 FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS build
 
 RUN apt update && apt upgrade -y && apt install -y \
-        ros-$ROS_DISTRO-nav2-msgs \
+        ros-$ROS_DISTRO-nav2-msgs
 
 # Create healthcheck package
 WORKDIR /ros2_ws

From 2b220401322b4812fe1266fbf490f1f746fd7e51 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Fri, 17 Nov 2023 18:32:25 +0100
Subject: [PATCH 14/25] Update Dockerfile

---
 Dockerfile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Dockerfile b/Dockerfile
index 9e22bb5..622101b 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -4,7 +4,7 @@ ARG PREFIX=
 ### BUILD IMAGE ###
 FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS build
 
-RUN apt update && apt upgrade -y && apt install -y \
+RUN apt update && apt install -y \
         ros-$ROS_DISTRO-nav2-msgs
 
 # Create healthcheck package

From 3f3afc6dee84be48a1ceff172515aa8ef08e642f Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 19:00:54 +0100
Subject: [PATCH 15/25] fix

---
 Dockerfile                             | 2 +-
 healthcheck/healthcheck_navigation.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 622101b..db53f48 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -54,7 +54,7 @@ RUN apt update && apt upgrade -y && apt install -y \
     rm -rf /var/lib/apt/lists/*
 
 COPY ./nav2_params /nav2_params
-COPY --from=build /ros2_ws /ros2_ws
+COPY --from=build /ros2_ws/install /ros2_ws/install
 
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
index d43b9f7..93e5501 100644
--- a/healthcheck/healthcheck_navigation.cpp
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -13,7 +13,7 @@ void write_health_status(const std::string &status) {
 int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("healthcheck_node");
-  std::string node_to_find = "/amcl";
+  std::string node_to_find = "/controller_server";
 
   while (rclcpp::ok()) {
     rclcpp::spin_some(node);

From 13416e6e3a32f93e83f80957e725d0898dad0d53 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 19:48:22 +0100
Subject: [PATCH 16/25] Fix first healthcheck running

---
 Dockerfile                 | 2 +-
 healthcheck/healthcheck.sh | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index db53f48..38e909f 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -59,7 +59,7 @@ COPY --from=build /ros2_ws/install /ros2_ws/install
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
 COPY ./healthcheck/healthcheck.sh /
-HEALTHCHECK --interval=10s --timeout=3s --start-period=5s --retries=5 \
+HEALTHCHECK --interval=10s --timeout=8s --start-period=5s --retries=5 \
     CMD ["/healthcheck.sh"]
 
 #tip: gathering logs from healthcheck: docker inspect b39 | jq '.[0].State.Health.Log'
diff --git a/healthcheck/healthcheck.sh b/healthcheck/healthcheck.sh
index dee593c..bacbde7 100755
--- a/healthcheck/healthcheck.sh
+++ b/healthcheck/healthcheck.sh
@@ -11,7 +11,7 @@ if [ ! -f "$HEALTHCHECK_FILE" ]; then
     echo "Healthcheck file not found. Starting ROS 2 healthcheck node..."
     start_healthcheck_node
     # Wait a bit to allow the node to start and write its initial status
-    sleep 2
+    sleep 5
 fi
 
 # Now check the health status

From a3b9ed9898791922e9530f3873cde0ba2d47acc4 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 23:16:21 +0100
Subject: [PATCH 17/25] Add entrypoint

---
 Dockerfile                 |  4 +++-
 healthcheck/healthcheck.sh | 12 ------------
 ros_entrypoint.sh          | 16 ++++++++++++++++
 3 files changed, 19 insertions(+), 13 deletions(-)
 create mode 100755 ros_entrypoint.sh

diff --git a/Dockerfile b/Dockerfile
index 38e909f..409f781 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -58,8 +58,10 @@ COPY --from=build /ros2_ws/install /ros2_ws/install
 
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
+COPY ./ros_entrypoint.sh /
+
 COPY ./healthcheck/healthcheck.sh /
-HEALTHCHECK --interval=10s --timeout=8s --start-period=5s --retries=5 \
+HEALTHCHECK --interval=7s --timeout=2s  --start-period=5s --retries=5 \
     CMD ["/healthcheck.sh"]
 
 #tip: gathering logs from healthcheck: docker inspect b39 | jq '.[0].State.Health.Log'
diff --git a/healthcheck/healthcheck.sh b/healthcheck/healthcheck.sh
index bacbde7..90152e4 100755
--- a/healthcheck/healthcheck.sh
+++ b/healthcheck/healthcheck.sh
@@ -2,18 +2,6 @@
 
 HEALTHCHECK_FILE="/health_status.txt"
 
-# Function to start the ROS 2 healthcheck node
-start_healthcheck_node() {
-    /ros_entrypoint.sh ros2 run healthcheck_pkg healthcheck_$SLAM_MODE &
-}
-
-if [ ! -f "$HEALTHCHECK_FILE" ]; then
-    echo "Healthcheck file not found. Starting ROS 2 healthcheck node..."
-    start_healthcheck_node
-    # Wait a bit to allow the node to start and write its initial status
-    sleep 5
-fi
-
 # Now check the health status
 if [ -f "$HEALTHCHECK_FILE" ]; then
     status=$(cat "$HEALTHCHECK_FILE")
diff --git a/ros_entrypoint.sh b/ros_entrypoint.sh
new file mode 100755
index 0000000..664e304
--- /dev/null
+++ b/ros_entrypoint.sh
@@ -0,0 +1,16 @@
+#!/bin/bash
+set -e
+
+# Run husarnet-dds singleshot and capture the output
+output=$(husarnet-dds singleshot || true)
+[[ "$HUSARNET_DDS_DEBUG" == "TRUE" ]] && echo "$output"
+
+# Setup ROS environment
+source "/opt/ros/$ROS_DISTRO/setup.bash"
+test -f "/ros2_ws/install/setup.bash" && source "/ros2_ws/install/setup.bash"
+
+# Run healthcheck in the background
+ros2 run healthcheck_pkg "healthcheck_$SLAM_MODE" &
+
+# Execute additional commands
+exec "$@"

From 80fd1303eaa0b565a6dc5589c30b78dff3139f61 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Fri, 17 Nov 2023 23:26:08 +0100
Subject: [PATCH 18/25] Delete debug msg

---
 healthcheck/healthcheck_localization.cpp | 4 ----
 healthcheck/healthcheck_navigation.cpp   | 4 ----
 healthcheck/healthcheck_slam.cpp         | 2 --
 3 files changed, 10 deletions(-)

diff --git a/healthcheck/healthcheck_localization.cpp b/healthcheck/healthcheck_localization.cpp
index d43b9f7..97e6fd7 100755
--- a/healthcheck/healthcheck_localization.cpp
+++ b/healthcheck/healthcheck_localization.cpp
@@ -22,13 +22,9 @@ int main(int argc, char *argv[]) {
                                      node_to_find) != node_names.end();
 
     if (is_node_running) {
-      RCLCPP_INFO(node->get_logger(), "The %s node is running.",
-                  node_to_find.c_str());
       write_health_status("healthy");
 
     } else {
-      RCLCPP_WARN(node->get_logger(), "The %s node is not running.",
-                  node_to_find.c_str());
       write_health_status("unhealthy");
     }
     std::this_thread::sleep_for(LOOP_PERIOD);
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
index 93e5501..5a99c97 100644
--- a/healthcheck/healthcheck_navigation.cpp
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -22,13 +22,9 @@ int main(int argc, char *argv[]) {
                                      node_to_find) != node_names.end();
 
     if (is_node_running) {
-      RCLCPP_INFO(node->get_logger(), "The %s node is running.",
-                  node_to_find.c_str());
       write_health_status("healthy");
 
     } else {
-      RCLCPP_WARN(node->get_logger(), "The %s node is not running.",
-                  node_to_find.c_str());
       write_health_status("unhealthy");
     }
     std::this_thread::sleep_for(LOOP_PERIOD);
diff --git a/healthcheck/healthcheck_slam.cpp b/healthcheck/healthcheck_slam.cpp
index 5695a88..30a28ae 100755
--- a/healthcheck/healthcheck_slam.cpp
+++ b/healthcheck/healthcheck_slam.cpp
@@ -35,10 +35,8 @@ class HealthCheckNode : public rclcpp::Node {
         saved_map_elapsed_time.count() < SAVE_MAP_VALID_TIME.count();
 
     if (is_msg_valid && is_save_map_valid) {
-      RCLCPP_INFO(get_logger(), "Health check: healthy");
       writeHealthStatus("healthy");
     } else {
-      RCLCPP_WARN(get_logger(), "Health check: unhealthy");
       writeHealthStatus("unhealthy");
     }
   }

From 2268a7f7eaf30544da3dbd18c4af0d9513602098 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Mon, 20 Nov 2023 15:18:00 +0100
Subject: [PATCH 19/25] Update Dockerfile

---
 Dockerfile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Dockerfile b/Dockerfile
index 409f781..0b07442 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -48,7 +48,7 @@ RUN apt update && apt upgrade -y && apt install -y \
         ros-$ROS_DISTRO-navigation2 \
         ros-$ROS_DISTRO-nav2-bringup \
         ros-$ROS_DISTRO-pointcloud-to-laserscan && \
-    mkdir maps && \
+    mkdir /maps && \
     apt-get autoremove -y && \
     apt-get clean && \
     rm -rf /var/lib/apt/lists/*

From 986f6d0dd235473d33d7cb5189107d373b9cf636 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Mon, 20 Nov 2023 17:50:08 +0100
Subject: [PATCH 20/25] Delete ros_entrypoint

---
 Dockerfile        |  7 ++++---
 ros_entrypoint.sh | 16 ----------------
 2 files changed, 4 insertions(+), 19 deletions(-)
 delete mode 100755 ros_entrypoint.sh

diff --git a/Dockerfile b/Dockerfile
index 0b07442..00e8fd9 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -14,10 +14,9 @@ RUN mkdir src && cd src/ && \
     source /opt/ros/$ROS_DISTRO/setup.bash && \
     ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp std_msgs && \
     sed -i '/find_package(std_msgs REQUIRED)/a \
-            find_package(geometry_msgs REQUIRED)\n \
             find_package(nav2_msgs REQUIRED)\n \
             add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
-            ament_target_dependencies(healthcheck_localization rclcpp geometry_msgs)\n \
+            ament_target_dependencies(healthcheck_localization rclcpp)\n \
             add_executable(healthcheck_navigation src/healthcheck_navigation.cpp)\n \
             ament_target_dependencies(healthcheck_navigation rclcpp)\n \
             add_executable(healthcheck_slam src/healthcheck_slam.cpp)\n \
@@ -58,7 +57,9 @@ COPY --from=build /ros2_ws/install /ros2_ws/install
 
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
-COPY ./ros_entrypoint.sh /
+RUN sed -i '/test -f "\/ros2_ws\/install\/setup.bash" && source "\/ros2_ws\/install\/setup.bash"/a \
+        ros2 run healthcheck_pkg "healthcheck_$SLAM_MODE" &\
+        /ros_entrypoint.sh
 
 COPY ./healthcheck/healthcheck.sh /
 HEALTHCHECK --interval=7s --timeout=2s  --start-period=5s --retries=5 \
diff --git a/ros_entrypoint.sh b/ros_entrypoint.sh
deleted file mode 100755
index 664e304..0000000
--- a/ros_entrypoint.sh
+++ /dev/null
@@ -1,16 +0,0 @@
-#!/bin/bash
-set -e
-
-# Run husarnet-dds singleshot and capture the output
-output=$(husarnet-dds singleshot || true)
-[[ "$HUSARNET_DDS_DEBUG" == "TRUE" ]] && echo "$output"
-
-# Setup ROS environment
-source "/opt/ros/$ROS_DISTRO/setup.bash"
-test -f "/ros2_ws/install/setup.bash" && source "/ros2_ws/install/setup.bash"
-
-# Run healthcheck in the background
-ros2 run healthcheck_pkg "healthcheck_$SLAM_MODE" &
-
-# Execute additional commands
-exec "$@"

From 259258e30a3be8b2dd65fb7a5ad27398f754d66a Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Mon, 20 Nov 2023 18:00:00 +0100
Subject: [PATCH 21/25] final fix

---
 Dockerfile                               |  7 +++---
 healthcheck/healthcheck_localization.cpp | 29 +++++++++++-------------
 healthcheck/healthcheck_navigation.cpp   | 29 +++++++++++-------------
 3 files changed, 30 insertions(+), 35 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 00e8fd9..b8f5e75 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -14,11 +14,12 @@ RUN mkdir src && cd src/ && \
     source /opt/ros/$ROS_DISTRO/setup.bash && \
     ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp std_msgs && \
     sed -i '/find_package(std_msgs REQUIRED)/a \
+            find_package(lifecycle_msgs REQUIRED)\n \
             find_package(nav2_msgs REQUIRED)\n \
             add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
-            ament_target_dependencies(healthcheck_localization rclcpp)\n \
+            ament_target_dependencies(healthcheck_localization rclcpp lifecycle_msgs)\n \
             add_executable(healthcheck_navigation src/healthcheck_navigation.cpp)\n \
-            ament_target_dependencies(healthcheck_navigation rclcpp)\n \
+            ament_target_dependencies(healthcheck_navigation rclcpp lifecycle_msgs)\n \
             add_executable(healthcheck_slam src/healthcheck_slam.cpp)\n \
             ament_target_dependencies(healthcheck_slam rclcpp nav2_msgs)\n \
             install(TARGETS \
@@ -58,7 +59,7 @@ COPY --from=build /ros2_ws/install /ros2_ws/install
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
 RUN sed -i '/test -f "\/ros2_ws\/install\/setup.bash" && source "\/ros2_ws\/install\/setup.bash"/a \
-        ros2 run healthcheck_pkg "healthcheck_$SLAM_MODE" &\
+        ros2 run healthcheck_pkg "healthcheck_$SLAM_MODE" &' \
         /ros_entrypoint.sh
 
 COPY ./healthcheck/healthcheck.sh /
diff --git a/healthcheck/healthcheck_localization.cpp b/healthcheck/healthcheck_localization.cpp
index 97e6fd7..1126228 100755
--- a/healthcheck/healthcheck_localization.cpp
+++ b/healthcheck/healthcheck_localization.cpp
@@ -1,34 +1,31 @@
 #include "fstream"
+#include "lifecycle_msgs/msg/transition_event.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
 
-#define LOOP_PERIOD 2s
-
 void write_health_status(const std::string &status) {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
 }
 
+void msg_callback(const lifecycle_msgs::msg::TransitionEvent::SharedPtr msg) {
+  if (msg->goal_state.label == "active") {
+    write_health_status("healthy");
+  } else {
+    write_health_status("unhealthy");
+  }
+}
+
 int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("healthcheck_node");
-  std::string node_to_find = "/amcl";
-
-  while (rclcpp::ok()) {
-    rclcpp::spin_some(node);
-    auto node_names = node->get_node_names();
-    auto is_node_running = std::find(node_names.begin(), node_names.end(),
-                                     node_to_find) != node_names.end();
 
-    if (is_node_running) {
-      write_health_status("healthy");
+  auto sub = node->create_subscription<lifecycle_msgs::msg::TransitionEvent>(
+      "amcl/transition_event", rclcpp::SystemDefaultsQoS(),
+      msg_callback);
 
-    } else {
-      write_health_status("unhealthy");
-    }
-    std::this_thread::sleep_for(LOOP_PERIOD);
-  }
+  rclcpp::spin(node);
 
   return 0;
 }
diff --git a/healthcheck/healthcheck_navigation.cpp b/healthcheck/healthcheck_navigation.cpp
index 5a99c97..b4b98a7 100644
--- a/healthcheck/healthcheck_navigation.cpp
+++ b/healthcheck/healthcheck_navigation.cpp
@@ -1,34 +1,31 @@
 #include "fstream"
+#include "lifecycle_msgs/msg/transition_event.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
 
-#define LOOP_PERIOD 2s
-
 void write_health_status(const std::string &status) {
   std::ofstream healthFile("/health_status.txt");
   healthFile << status;
 }
 
+void msg_callback(const lifecycle_msgs::msg::TransitionEvent::SharedPtr msg) {
+  if (msg->goal_state.label == "active") {
+    write_health_status("healthy");
+  } else {
+    write_health_status("unhealthy");
+  }
+}
+
 int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("healthcheck_node");
-  std::string node_to_find = "/controller_server";
-
-  while (rclcpp::ok()) {
-    rclcpp::spin_some(node);
-    auto node_names = node->get_node_names();
-    auto is_node_running = std::find(node_names.begin(), node_names.end(),
-                                     node_to_find) != node_names.end();
 
-    if (is_node_running) {
-      write_health_status("healthy");
+  auto sub = node->create_subscription<lifecycle_msgs::msg::TransitionEvent>(
+      "controller_server/transition_event", rclcpp::SystemDefaultsQoS(),
+      msg_callback);
 
-    } else {
-      write_health_status("unhealthy");
-    }
-    std::this_thread::sleep_for(LOOP_PERIOD);
-  }
+  rclcpp::spin(node);
 
   return 0;
 }

From 9353ab2dd5e361f526406d4a880b9e8c535074c2 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 21 Nov 2023 14:28:31 +0100
Subject: [PATCH 22/25] delete std_pkg

---
 Dockerfile | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index b8f5e75..bfc5ec6 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -12,10 +12,8 @@ WORKDIR /ros2_ws
 
 RUN mkdir src && cd src/ && \
     source /opt/ros/$ROS_DISTRO/setup.bash && \
-    ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp std_msgs && \
-    sed -i '/find_package(std_msgs REQUIRED)/a \
-            find_package(lifecycle_msgs REQUIRED)\n \
-            find_package(nav2_msgs REQUIRED)\n \
+    ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp lifecycle_msgs nav2_msgs && \
+    sed -i '/find_package(nav2_msgs REQUIRED)/a \
             add_executable(healthcheck_localization src/healthcheck_localization.cpp)\n \
             ament_target_dependencies(healthcheck_localization rclcpp lifecycle_msgs)\n \
             add_executable(healthcheck_navigation src/healthcheck_navigation.cpp)\n \

From 239b98ac0a8f8a288193170e0f9631890a74b263 Mon Sep 17 00:00:00 2001
From: dominikn <dominik.nwk@gmail.com>
Date: Wed, 22 Nov 2023 12:01:46 +0100
Subject: [PATCH 23/25] building for iron

---
 .github/workflows/ros-docker-image.yaml        | 2 +-
 .github/workflows/vulcanexus-docker-image.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/ros-docker-image.yaml b/.github/workflows/ros-docker-image.yaml
index 4ed18c9..884b353 100644
--- a/.github/workflows/ros-docker-image.yaml
+++ b/.github/workflows/ros-docker-image.yaml
@@ -36,7 +36,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros-distro: [ "humble" ]
+        ros-distro: [ "humble" , "iron"]
 
     steps:
 
diff --git a/.github/workflows/vulcanexus-docker-image.yaml b/.github/workflows/vulcanexus-docker-image.yaml
index 67c90ac..fd03679 100644
--- a/.github/workflows/vulcanexus-docker-image.yaml
+++ b/.github/workflows/vulcanexus-docker-image.yaml
@@ -36,7 +36,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros-distro: [ "humble" ]
+        ros-distro: [ "humble", "iron" ]
 
     steps:
 

From 654980235ec39c3160941266125aa1691d303d66 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Mon, 27 Nov 2023 10:50:13 +0100
Subject: [PATCH 24/25] Fix Vulcannexus healthcheck

---
 Dockerfile | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index bfc5ec6..70b4e46 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -56,9 +56,15 @@ COPY --from=build /ros2_ws/install /ros2_ws/install
 
 RUN echo $(dpkg -s ros-$ROS_DISTRO-navigation2 | grep 'Version' | sed -r 's/Version:\s([0-9]+.[0-9]+.[0-9]+).*/\1/g') > /version.txt
 
-RUN sed -i '/test -f "\/ros2_ws\/install\/setup.bash" && source "\/ros2_ws\/install\/setup.bash"/a \
+RUN if [ -f "/ros_entrypoint.sh" ]; then \
+        sed -i '/test -f "\/ros2_ws\/install\/setup.bash" && source "\/ros2_ws\/install\/setup.bash"/a \
         ros2 run healthcheck_pkg "healthcheck_$SLAM_MODE" &' \
-        /ros_entrypoint.sh
+        /ros_entrypoint.sh; \
+    else \
+        sed -i '/test -f "\/ros2_ws\/install\/setup.bash" && source "\/ros2_ws\/install\/setup.bash"/a \
+        ros2 run healthcheck_pkg "healthcheck_$SLAM_MODE" &' \
+        /vulcanexus_entrypoint.sh; \
+    fi
 
 COPY ./healthcheck/healthcheck.sh /
 HEALTHCHECK --interval=7s --timeout=2s  --start-period=5s --retries=5 \

From e89abb89f5c4d2f0a10b28bdd831791cb0d2de3a Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Mon, 27 Nov 2023 11:38:46 +0100
Subject: [PATCH 25/25] iron

---
 .github/workflows/ros-docker-image.yaml        | 2 +-
 .github/workflows/vulcanexus-docker-image.yaml | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/ros-docker-image.yaml b/.github/workflows/ros-docker-image.yaml
index 884b353..003e1f5 100644
--- a/.github/workflows/ros-docker-image.yaml
+++ b/.github/workflows/ros-docker-image.yaml
@@ -36,7 +36,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros-distro: [ "humble" , "iron"]
+        ros-distro: [ "humble" ] # iron is not supported on arm
 
     steps:
 
diff --git a/.github/workflows/vulcanexus-docker-image.yaml b/.github/workflows/vulcanexus-docker-image.yaml
index fd03679..dc5f2d0 100644
--- a/.github/workflows/vulcanexus-docker-image.yaml
+++ b/.github/workflows/vulcanexus-docker-image.yaml
@@ -1,6 +1,6 @@
-name: Build/Publish Vulcanexus Docker Image 
+name: Build/Publish Vulcanexus Docker Image
 
-on: 
+on:
   workflow_dispatch:
     inputs:
       build_type:
@@ -36,7 +36,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros-distro: [ "humble", "iron" ]
+        ros-distro: [ "humble" ] # iron is not supported on arm
 
     steps: