diff --git a/.github/workflows/ros-docker-image.yaml b/.github/workflows/ros-docker-image.yaml index 929b8ce..34cce4c 100644 --- 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 index ee88e7a..20d9500 100644 --- 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 index c66f76f..d33fe6c 100644 --- 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 '' | sed -r 's/.*([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 index ccd6017..efcd365 100644 --- 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 '' | sed -r 's/.*([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 index 0e5adcd..89bb0eb 100755 --- 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,7 +44,6 @@ 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) @@ -53,7 +51,6 @@ def enter_bootloader_mode(self): time.sleep(0.2) def exit_bootloader_mode(self): - self.boot0_pin.write(False) self.reset_pin.write(True) time.sleep(0.2) @@ -61,41 +58,48 @@ def exit_bootloader_mode(self): 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 new file mode 100644 index 0000000..1e032e3 --- /dev/null +++ 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 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( + "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 new file mode 100755 index 0000000..e25879f --- /dev/null +++ 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 index 7738574..01a34bf 100755 --- 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(