Add healthcheck (#39)

* Add healthcheck to simulation * Add healthcheck hardware * Timeout * New version of Healthcheck * fix * timeout * Fix first healthcheck running * Add entrypoint * Delete debug msg * refactor * Delete ros_entrypoint * Update ros-docker-image.yaml * delete std_msgs * fix flash rw unprotecting * small ux change in flash rw unprotecting Signed-off-by: Jan Brzyk <[email protected]> * small firmware flashing fix Signed-off-by: Jan Brzyk <[email protected]> * firmware flashing fix Signed-off-by: Jan Brzyk <[email protected]> * Fix Vulcannexus healthcheck --------- Signed-off-by: Jan Brzyk <[email protected]> Co-authored-by: dominikn <[email protected]> Co-authored-by: Jan Brzyk <[email protected]>
husarion · Nov 27, 2023 · f5a70da · f5a70da
1 parent 7f18f3f
commit f5a70da
Show file tree

Hide file tree

Showing 8 changed files with 161 additions and 50 deletions.
diff --git a/.github/workflows/ros-docker-image.yaml b/.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:
 

diff --git a/.pre-commit-config.yaml b/.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
diff --git a/Dockerfile.hardware b/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/

diff --git a/Dockerfile.simulation b/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"]
diff --git a/flash-firmware.py b/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__":

diff --git a/healthcheck.cpp b/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;
+}
diff --git a/healthcheck.sh b/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
diff --git a/print-serial-number.py b/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(