Add healthcheck

.github/workflows/ros-docker-image.yaml

@@ -51,10 +51,10 @@ jobs:
                       repo_name: ''
                       platforms: linux/amd64, linux/arm64
                       ros_distro: iron
-                    - dockerfile: Dockerfile.simulation
-                      repo_name: rosbot-gazebo
-                      platforms: linux/amd64
-                      ros_distro: iron
+                    # - dockerfile: Dockerfile.simulation
+                    #   repo_name: rosbot-gazebo
+                    #   platforms: linux/amd64
+                    #   ros_distro: iron
 
         steps:
 

.pre-commit-config.yaml

@@ -16,7 +16,7 @@ repos:
             args: [--pytest-test-first]
 
     - repo: https://github.com/codespell-project/codespell
-      rev: v1.16.0
+      rev: v2.2.6
       hooks:
           - id: codespell
             name: codespell
@@ -26,13 +26,13 @@ repos:
             types: [text]
 
     - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt
-      rev: 0.2.1
+      rev: 0.2.3
       hooks:
           - id: yamlfmt
             files: ^.github|./\.yaml
 
     - repo: https://github.com/psf/black
-      rev: 22.12.0
+      rev: 23.11.0
       hooks:
         - id: black
           args: ["--line-length=99"]
@@ -51,7 +51,8 @@ repos:
         - id: doc8
           args: ['--max-line-length=100', '--ignore=D001']
           exclude: ^.*\/CHANGELOG\.rst/.*$
+
     - repo: https://github.com/pre-commit/mirrors-clang-format
-      rev: v16.0.6
+      rev: v17.0.4
       hooks:
         -   id: clang-format

Dockerfile.hardware

@@ -67,6 +67,7 @@ COPY --from=stm32flash_builder_and_downloader /firmware.hex /root/firmware.hex
 COPY --from=stm32flash_builder_and_downloader /stm32flash/stm32flash /usr/bin/stm32flash
 COPY --from=stm32flash_builder_and_downloader /ros2_ws /ros2_ws
 COPY --from=cpu_id_builder /read_cpu_id/.pio/build/olimex_e407/firmware.bin /firmware_read_cpu_id.bin
+COPY ./healthcheck.cpp /
 
 RUN apt-get update && apt-get install -y \
 		git \
@@ -88,7 +89,18 @@ RUN apt-get update && apt-get install -y \
 	rosdep init && \
     rosdep update --rosdistro $ROS_DISTRO && \
     rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \
+	# Create health check package
+    cd src/ && \
 	source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \
+    ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs && \
+    sed -i '/find_package(nav_msgs REQUIRED)/a \
+            add_executable(healthcheck_node src/healthcheck.cpp)\n \
+            ament_target_dependencies(healthcheck_node rclcpp nav_msgs)\n \
+            install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \
+            /ros2_ws/src/healthcheck_pkg/CMakeLists.txt && \
+    mv /healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ && \
+    cd .. && \
+    # Build
 	colcon build && \
 	# clear ubuntu packages
 	apt-get clean && \
@@ -103,6 +115,20 @@ RUN apt-get update && apt-get install -y \
 
 RUN echo $(cat /ros2_ws/src/rosbot/package.xml | grep '<version>' | sed -r 's/.*<version>([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt
 
+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_node &' \
+        /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_node &' \
+        /vulcanexus_entrypoint.sh; \
+    fi
+
+COPY ./healthcheck.sh /
+HEALTHCHECK --interval=7s --timeout=2s  --start-period=5s --retries=5 \
+    CMD ["/healthcheck.sh"]
+
 # copy scripts
 COPY flash-firmware.py /
 COPY flash-firmware.py /usr/bin/

Dockerfile.simulation

@@ -13,40 +13,57 @@ ENV HUSARION_ROS_BUILD simulation
 
 WORKDIR /ros2_ws
 
+COPY ./healthcheck.cpp /
+
 # install everything needed
 RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \
     apt-get update --fix-missing && apt-get install -y \
-        python3-pip \
-        python3-colcon-common-extensions \
-        python3-rosdep \
-        python3-vcstool \
-        git \
-        curl \
-        ros-$ROS_DISTRO-teleop-twist-keyboard \
-        && \
-    apt-get upgrade -y && \
-    # build & install ROSbot 2 packages
+        ros-dev-tools && \
+    # Clone source
     source "/opt/$MYDISTRO/$ROS_DISTRO/setup.bash" && \
     git clone https://github.com/husarion/rosbot_ros.git /ros2_ws/src && \
     vcs import src < src/rosbot/rosbot_hardware.repos && \
     vcs import src < src/rosbot/rosbot_simulation.repos && \
-    # without this line (using vulcanexus base image) rosdep init throws error: "ERROR: default sources list file already exists:"
+    # Install dependencies
     rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \
     rosdep init && \
     rosdep update --rosdistro $ROS_DISTRO && \
     rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \
+    # Create healthcheck package
+    cd src/ && \
+	source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \
+    ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs && \
+    sed -i '/find_package(nav_msgs REQUIRED)/a \
+            add_executable(healthcheck_node src/healthcheck.cpp)\n \
+            ament_target_dependencies(healthcheck_node rclcpp nav_msgs)\n \
+            install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \
+            /ros2_ws/src/healthcheck_pkg/CMakeLists.txt && \
+    mv /healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ && \
+    cd .. && \
+    # Build
     colcon build && \
     # make the image smaller
     export SUDO_FORCE_REMOVE=yes && \
     apt-get remove -y \
         python3-pip \
-        python3-colcon-common-extensions \
-        python3-rosdep \
-        python3-vcstool \
-        git \
+        ros-dev-tools \
         curl && \
     apt-get autoremove -y && \
     apt-get clean && \
     rm -rf /var/lib/apt/lists/*
 
 RUN echo $(cat /ros2_ws/src/rosbot_gazebo/package.xml | grep '<version>' | sed -r 's/.*<version>([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') > /version.txt
+
+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_node &' \
+        /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_node &' \
+        /vulcanexus_entrypoint.sh; \
+    fi
+
+COPY ./healthcheck.sh /
+HEALTHCHECK --interval=7s --timeout=2s  --start-period=5s --retries=5 \
+    CMD ["/healthcheck.sh"]

flash-firmware.py

@@ -9,7 +9,6 @@
 
 class FirmwareFlasher:
     def __init__(self, sys_arch, binary_file):
-
         self.binary_file = binary_file
         self.sys_arch = sys_arch
 
@@ -45,57 +44,62 @@ def __init__(self, sys_arch, binary_file):
         self.reset_pin = GPIO(reset_pin_no, "out")
 
     def enter_bootloader_mode(self):
-
         self.boot0_pin.write(True)
         self.reset_pin.write(True)
         time.sleep(0.2)
         self.reset_pin.write(False)
         time.sleep(0.2)
 
     def exit_bootloader_mode(self):
-
         self.boot0_pin.write(False)
         self.reset_pin.write(True)
         time.sleep(0.2)
         self.reset_pin.write(False)
         time.sleep(0.2)
 
     def flash_firmware(self):
-
         self.enter_bootloader_mode()
 
-        # Flashing the firmware
-        succes_no = 0
+        # Disable the flash write-protection
+        for i in range(self.max_approach_no):
+            try:
+                sh.stm32flash(self.port, "-u", _out=sys.stdout)
+                time.sleep(0.2)
+                break
+            except Exception:
+                print("Write-UnProtection error! Trying again.")
+                pass
+        else:
+            print("WARNING! Disabling the flash Write-Protection went wrong.")
+
+        # Disable the flash read-protection
         for i in range(self.max_approach_no):
             try:
-                if succes_no == 0:
-                    # Disable the flash write-protection
-                    sh.stm32flash(self.port, "-u", _out=sys.stdout)
-                    time.sleep(0.2)
-                    succes_no += 1
-
-                if succes_no == 1:
-                    # Disable the flash read-protection
-                    sh.stm32flash(self.port, "-k", _out=sys.stdout)
-                    time.sleep(0.2)
-                    succes_no += 1
-
-                if succes_no == 2:
-                    # Flashing the firmware
-                    sh.stm32flash(self.port, "-v", w=self.binary_file, b="115200", _out=sys.stdout)
-                    time.sleep(0.2)
-                    break
+                sh.stm32flash(self.port, "-k", _out=sys.stdout)
+                time.sleep(0.2)
+                break
             except Exception:
+                print("Read-UnProtection error! Trying again.")
                 pass
+        else:
+            print("WARNING! Disabling the flash Read-Protection went wrong.")
 
+        # Flashing the firmware
+        for i in range(self.max_approach_no):
+            try:
+                sh.stm32flash(self.port, "-v", w=self.binary_file, b="115200", _out=sys.stdout)
+                time.sleep(0.2)
+                break
+            except Exception:
+                print("Flashing error! Trying again.")
+                pass
         else:
-            print("ERROR! Something goes wrong. Try again.")
+            print("ERROR! Flashing the firmware went wrong. Try again.")
 
         self.exit_bootloader_mode()
 
 
 def main():
-
     parser = argparse.ArgumentParser(
         description="Flashing the firmware on STM32 microcontroller in ROSbot"
     )
@@ -112,7 +116,7 @@ def main():
 
     flasher = FirmwareFlasher(sys_arch, binary_file)
     flasher.flash_firmware()
-    print("Done.")
+    print("Done!")
 
 
 if __name__ == "__main__":

healthcheck.cpp

@@ -0,0 +1,47 @@
+#include "fstream"
+#include "nav_msgs/msg/odometry.hpp"
+#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 nav_msgs::msg::Odometry::SharedPtr msg) {
+  last_msg_time = std::chrono::steady_clock::now();
+}
+
+void healthy_check() {
+  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) {
+    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");
+  auto sub = node->create_subscription<nav_msgs::msg::Odometry>(
+      "odometry/filtered", rclcpp::SensorDataQoS(), msg_callback);
+
+  while (rclcpp::ok()) {
+    rclcpp::spin_some(node);
+    healthy_check();
+    std::this_thread::sleep_for(LOOP_PERIOD);
+  }
+
+  return 0;
+}

healthcheck.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+HEALTHCHECK_FILE="/health_status.txt"
+
+
+# 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

print-serial-number.py

@@ -107,7 +107,6 @@ def generate(self):
 
 
 def main():
-
     parser = argparse.ArgumentParser(description="Printing ROSbot 2R / 2 PRO serial number")
 
     parser.add_argument(