From 55ac8a818122cb6b194f2b1e44a84108c022a4fe Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 7 Nov 2023 10:24:21 +0100 Subject: [PATCH 01/18] Add healthcheck to simulation --- Dockerfile.simulation | 20 +++++++++++++++++++- demo/compose.simulation.yaml | 2 +- healthcheck.cpp | 34 ++++++++++++++++++++++++++++++++++ 3 files changed, 54 insertions(+), 2 deletions(-) create mode 100755 healthcheck.cpp diff --git a/Dockerfile.simulation b/Dockerfile.simulation index ec0391e..f2f29a2 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -13,6 +13,8 @@ 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 \ @@ -35,6 +37,19 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 std_msgs && \ + sed -i '/find_package(std_msgs REQUIRED)/a \ + find_package(nav_msgs REQUIRED)\n \ + add_executable(healthcheck_node src/healthcheck.cpp)\n \ + ament_target_dependencies(healthcheck_node rclcpp std_msgs 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 && \ @@ -49,4 +64,7 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 \ No newline at end of file +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 + +HEALTHCHECK --interval=10s --timeout=5s --start-period=5s --retries=5 \ + CMD ["/ros_entrypoint.sh", "ros2", "run", "healthcheck_pkg", "healthcheck_node"] diff --git a/demo/compose.simulation.yaml b/demo/compose.simulation.yaml index f2218b4..616a95c 100644 --- a/demo/compose.simulation.yaml +++ b/demo/compose.simulation.yaml @@ -25,4 +25,4 @@ services: volumes: - /tmp/.X11-unix:/tmp/.X11-unix:rw - /dev:/dev - command: ros2 launch rosbot_gazebo simulation.launch.py mecanum:=${MECANUM:-False} \ No newline at end of file + command: ros2 launch rosbot_gazebo simulation.launch.py mecanum:=${MECANUM:-False} diff --git a/healthcheck.cpp b/healthcheck.cpp new file mode 100755 index 0000000..ef09c0f --- /dev/null +++ b/healthcheck.cpp @@ -0,0 +1,34 @@ +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "cstdlib" + +using namespace std::chrono_literals; + +#define TOPIC_NAME "/odometry/filtered" +#define TIMEOUT 2s + +int msg_received = EXIT_FAILURE; + +void msg_callback(const nav_msgs::msg::Odometry::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(TOPIC_NAME, rclcpp::SensorDataQoS(), msg_callback); + auto timer = node->create_wall_timer(TIMEOUT, timeout_callback); + + rclcpp::spin(node); + return msg_received; +} From a449ef1eb9d69da0698362d70d7b3646d9c58b04 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 7 Nov 2023 11:02:13 +0100 Subject: [PATCH 02/18] Add healthcheck hardware --- Dockerfile.hardware | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/Dockerfile.hardware b/Dockerfile.hardware index c66f76f..274cecd 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,19 @@ 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 std_msgs && \ + sed -i '/find_package(std_msgs REQUIRED)/a \ + find_package(nav_msgs REQUIRED)\n \ + add_executable(healthcheck_node src/healthcheck.cpp)\n \ + ament_target_dependencies(healthcheck_node rclcpp std_msgs 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 +116,9 @@ 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 +HEALTHCHECK --interval=10s --timeout=5s --start-period=5s --retries=5 \ + CMD ["/ros_entrypoint.sh", "ros2", "run", "healthcheck_pkg", "healthcheck_node"] + # copy scripts COPY flash-firmware.py / COPY flash-firmware.py /usr/bin/ From 3cabf4a03c70f654d3f5c26a2b868f6c97a2d35f Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 13 Nov 2023 17:20:18 +0100 Subject: [PATCH 03/18] Timeout --- Dockerfile.hardware | 2 +- Dockerfile.simulation | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Dockerfile.hardware b/Dockerfile.hardware index 274cecd..7d735d2 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -116,7 +116,7 @@ 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 -HEALTHCHECK --interval=10s --timeout=5s --start-period=5s --retries=5 \ +HEALTHCHECK --interval=15s --timeout=10s --start-period=5s --retries=5 \ CMD ["/ros_entrypoint.sh", "ros2", "run", "healthcheck_pkg", "healthcheck_node"] # copy scripts diff --git a/Dockerfile.simulation b/Dockerfile.simulation index f2f29a2..7ee3a15 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -66,5 +66,5 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 -HEALTHCHECK --interval=10s --timeout=5s --start-period=5s --retries=5 \ +HEALTHCHECK --interval=15s --timeout=10s --start-period=5s --retries=5 \ CMD ["/ros_entrypoint.sh", "ros2", "run", "healthcheck_pkg", "healthcheck_node"] From 4e6c0611690c610f9e7ad0f76bd054d0aa1683f3 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 17 Nov 2023 14:26:01 +0100 Subject: [PATCH 04/18] New version of Healthcheck --- Dockerfile.hardware | 5 +++-- Dockerfile.simulation | 5 +++-- healthcheck.cpp | 44 ++++++++++++++++++++++++++++++------------- healthcheck.sh | 28 +++++++++++++++++++++++++++ 4 files changed, 65 insertions(+), 17 deletions(-) create mode 100755 healthcheck.sh diff --git a/Dockerfile.hardware b/Dockerfile.hardware index 7d735d2..baefd10 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -116,8 +116,9 @@ 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 -HEALTHCHECK --interval=15s --timeout=10s --start-period=5s --retries=5 \ - CMD ["/ros_entrypoint.sh", "ros2", "run", "healthcheck_pkg", "healthcheck_node"] +COPY ./healthcheck.sh / +HEALTHCHECK --interval=10s --timeout=2s --start-period=5s --retries=5 \ + CMD ["/healthcheck.sh"] # copy scripts COPY flash-firmware.py / diff --git a/Dockerfile.simulation b/Dockerfile.simulation index 7ee3a15..592c2a1 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -66,5 +66,6 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 -HEALTHCHECK --interval=15s --timeout=10s --start-period=5s --retries=5 \ - CMD ["/ros_entrypoint.sh", "ros2", "run", "healthcheck_pkg", "healthcheck_node"] +COPY ./healthcheck.sh / +HEALTHCHECK --interval=10s --timeout=2s --start-period=5s --retries=5 \ + CMD ["/healthcheck.sh"] diff --git a/healthcheck.cpp b/healthcheck.cpp index ad38354..eb2eefb 100644 --- a/healthcheck.cpp +++ b/healthcheck.cpp @@ -1,32 +1,50 @@ -#include "cstdlib" +#include "fstream" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" using namespace std::chrono_literals; -#define TOPIC_NAME "/odometry/filtered" -#define TIMEOUT 2s +#define LOOP_PERIOD 2s +#define MSG_VALID_TIME 5s -int msg_received = EXIT_FAILURE; +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) { std::cout << "Message received" << std::endl; - msg_received = EXIT_SUCCESS; - rclcpp::shutdown(); + last_msg_time = std::chrono::steady_clock::now(); } -void timeout_callback() { - std::cout << "Timeout" << std::endl; - rclcpp::shutdown(); +void healthy_check(const rclcpp::Node::SharedPtr &node) { + 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) { + std::cout << "Health check: healthy" << std::endl; + write_health_status("healthy"); + } else { + std::cout << "Health check: unhealthy" << std::endl; + 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( - TOPIC_NAME, rclcpp::SensorDataQoS(), msg_callback); - auto timer = node->create_wall_timer(TIMEOUT, timeout_callback); + "/odometry/filtered", rclcpp::SensorDataQoS(), msg_callback); + + while (rclcpp::ok()) { + rclcpp::spin_some(node); + healthy_check(node); + std::this_thread::sleep_for(LOOP_PERIOD); + } - rclcpp::spin(node); - return msg_received; + return 0; } diff --git a/healthcheck.sh b/healthcheck.sh new file mode 100755 index 0000000..2aacdc9 --- /dev/null +++ b/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_node & +} + +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 From 53550dc0ac2a84ff23e4113760cc56747d210d40 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 17 Nov 2023 14:33:27 +0100 Subject: [PATCH 05/18] fix --- healthcheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/healthcheck.cpp b/healthcheck.cpp index eb2eefb..0c612ca 100644 --- a/healthcheck.cpp +++ b/healthcheck.cpp @@ -38,7 +38,7 @@ 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); + "odometry/filtered", rclcpp::SensorDataQoS(), msg_callback); while (rclcpp::ok()) { rclcpp::spin_some(node); From 20ff0cbc793955caf411f723513349c7a1839a83 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 17 Nov 2023 17:49:17 +0100 Subject: [PATCH 06/18] timeout --- Dockerfile.hardware | 2 +- Dockerfile.simulation | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Dockerfile.hardware b/Dockerfile.hardware index baefd10..d36a7bd 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -117,7 +117,7 @@ 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 COPY ./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"] # copy scripts diff --git a/Dockerfile.simulation b/Dockerfile.simulation index 592c2a1..fff15bc 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -67,5 +67,5 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 COPY ./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"] From 6721b508ff53a4000780d6a6a5a475f3b84c8f4f Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 17 Nov 2023 19:48:11 +0100 Subject: [PATCH 07/18] Fix first healthcheck running --- Dockerfile.hardware | 2 +- Dockerfile.simulation | 2 +- healthcheck.sh | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Dockerfile.hardware b/Dockerfile.hardware index d36a7bd..069dd0f 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -117,7 +117,7 @@ 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 COPY ./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"] # copy scripts diff --git a/Dockerfile.simulation b/Dockerfile.simulation index fff15bc..54f73f0 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -67,5 +67,5 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 COPY ./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"] diff --git a/healthcheck.sh b/healthcheck.sh index 2aacdc9..58af9cf 100755 --- a/healthcheck.sh +++ b/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 eeb1aeffa7a544d8741416fcf2d97d0bb76a3e31 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 17 Nov 2023 23:18:23 +0100 Subject: [PATCH 08/18] Add entrypoint --- Dockerfile.hardware | 4 +++- Dockerfile.simulation | 4 +++- healthcheck.sh | 11 ----------- ros_entrypoint.sh | 16 ++++++++++++++++ 4 files changed, 22 insertions(+), 13 deletions(-) create mode 100755 ros_entrypoint.sh diff --git a/Dockerfile.hardware b/Dockerfile.hardware index 069dd0f..e01c35b 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -116,8 +116,10 @@ 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 +COPY ./ros_entrypoint.sh / + COPY ./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"] # copy scripts diff --git a/Dockerfile.simulation b/Dockerfile.simulation index 54f73f0..a909d09 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -66,6 +66,8 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 +COPY ./ros_entrypoint.sh / + COPY ./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"] diff --git a/healthcheck.sh b/healthcheck.sh index 58af9cf..e25879f 100755 --- a/healthcheck.sh +++ b/healthcheck.sh @@ -2,17 +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_node & -} - -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 diff --git a/ros_entrypoint.sh b/ros_entrypoint.sh new file mode 100755 index 0000000..0799449 --- /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_node & + +# Execute additional commands +exec "$@" From b9cceb24cb55041cab495199e1b089e625585ac4 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 17 Nov 2023 23:26:21 +0100 Subject: [PATCH 09/18] Delete debug msg --- healthcheck.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/healthcheck.cpp b/healthcheck.cpp index 0c612ca..78f25c6 100644 --- a/healthcheck.cpp +++ b/healthcheck.cpp @@ -15,7 +15,6 @@ void write_health_status(const std::string &status) { } void msg_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - std::cout << "Message received" << std::endl; last_msg_time = std::chrono::steady_clock::now(); } @@ -26,10 +25,8 @@ void healthy_check(const rclcpp::Node::SharedPtr &node) { bool is_msg_valid = elapsed_time.count() < MSG_VALID_TIME.count(); if (is_msg_valid) { - std::cout << "Health check: healthy" << std::endl; write_health_status("healthy"); } else { - std::cout << "Health check: unhealthy" << std::endl; write_health_status("unhealthy"); } } From 0e1d1e566ac0f51cbb9871592bdce1b12cd32329 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 20 Nov 2023 16:27:43 +0100 Subject: [PATCH 10/18] refactor --- healthcheck.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/healthcheck.cpp b/healthcheck.cpp index 78f25c6..1e032e3 100644 --- a/healthcheck.cpp +++ b/healthcheck.cpp @@ -18,7 +18,7 @@ void msg_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { last_msg_time = std::chrono::steady_clock::now(); } -void healthy_check(const rclcpp::Node::SharedPtr &node) { +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; @@ -39,7 +39,7 @@ int main(int argc, char *argv[]) { while (rclcpp::ok()) { rclcpp::spin_some(node); - healthy_check(node); + healthy_check(); std::this_thread::sleep_for(LOOP_PERIOD); } From d1845cbfba8edf8700f8a87f424775757de4f80c Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 20 Nov 2023 17:36:25 +0100 Subject: [PATCH 11/18] Delete ros_entrypoint --- Dockerfile.hardware | 4 +++- Dockerfile.simulation | 4 +++- ros_entrypoint.sh | 16 ---------------- 3 files changed, 6 insertions(+), 18 deletions(-) delete mode 100755 ros_entrypoint.sh diff --git a/Dockerfile.hardware b/Dockerfile.hardware index e01c35b..dd40b9f 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -116,7 +116,9 @@ 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 -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_node &' \ + /ros_entrypoint.sh COPY ./healthcheck.sh / HEALTHCHECK --interval=7s --timeout=2s --start-period=5s --retries=5 \ diff --git a/Dockerfile.simulation b/Dockerfile.simulation index a909d09..ba15d2e 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -66,7 +66,9 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 -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_node &' \ + /ros_entrypoint.sh COPY ./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 0799449..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_node & - -# Execute additional commands -exec "$@" From 50233bff2e8050db0f36880534fcf9b459fa40ff Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Tue, 21 Nov 2023 11:55:33 +0100 Subject: [PATCH 12/18] Update ros-docker-image.yaml --- .github/workflows/ros-docker-image.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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: From 8b8197231b325a22e053b8c06b030f45943acdff Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 21 Nov 2023 14:27:25 +0100 Subject: [PATCH 13/18] delete std_msgs --- Dockerfile.hardware | 7 +++---- Dockerfile.simulation | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/Dockerfile.hardware b/Dockerfile.hardware index dd40b9f..7aa6faf 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -92,11 +92,10 @@ RUN apt-get update && apt-get install -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 std_msgs && \ - sed -i '/find_package(std_msgs REQUIRED)/a \ - find_package(nav_msgs REQUIRED)\n \ + 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 std_msgs nav_msgs)\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/ && \ diff --git a/Dockerfile.simulation b/Dockerfile.simulation index ba15d2e..5c1adb1 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -40,11 +40,10 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ # Create health check package cd src/ && \ source /opt/$MYDISTRO/$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(nav_msgs REQUIRED)\n \ + 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 std_msgs nav_msgs)\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/ && \ From ccc890c1553db1849e2b90cf83e44bdd237fc3af Mon Sep 17 00:00:00 2001 From: dominikn Date: Wed, 22 Nov 2023 15:11:49 +0100 Subject: [PATCH 14/18] fix flash rw unprotecting --- flash-firmware.py | 41 ++++++++++++++++++++++------------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/flash-firmware.py b/flash-firmware.py index 5e358ef..00b3359 100755 --- a/flash-firmware.py +++ b/flash-firmware.py @@ -60,30 +60,33 @@ def exit_bootloader_mode(self): 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: - 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, "-u", _out=sys.stdout) + time.sleep(0.2) except Exception: pass + else: + print("ERROR! Something goes wrong. Try again.") + # Disable the flash read-protection + for i in range(self.max_approach_no): + try: + sh.stm32flash(self.port, "-k", _out=sys.stdout) + time.sleep(0.2) + except Exception: + pass + else: + print("ERROR! Something goes wrong. Try again.") + + # 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) + except Exception: + pass else: print("ERROR! Something goes wrong. Try again.") From 08cb61cc3cd158eb764a3f8b66222e2ca0d444c8 Mon Sep 17 00:00:00 2001 From: Jan Brzyk Date: Thu, 23 Nov 2023 13:01:19 +0100 Subject: [PATCH 15/18] small ux change in flash rw unprotecting Signed-off-by: Jan Brzyk --- flash-firmware.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flash-firmware.py b/flash-firmware.py index 00b3359..6b595ad 100755 --- a/flash-firmware.py +++ b/flash-firmware.py @@ -68,7 +68,7 @@ def flash_firmware(self): except Exception: pass else: - print("ERROR! Something goes wrong. Try again.") + print("WARNING! Disabling the flash write-protection went wrong.") # Disable the flash read-protection for i in range(self.max_approach_no): @@ -78,7 +78,7 @@ def flash_firmware(self): except Exception: pass else: - print("ERROR! Something goes wrong. Try again.") + print("WARNING! Disabling the flash read-protection went wrong.") # Flashing the firmware for i in range(self.max_approach_no): @@ -88,7 +88,7 @@ def flash_firmware(self): except Exception: pass else: - print("ERROR! Something goes wrong. Try again.") + print("ERROR! Flashing the firmware went wrong. Try again.") self.exit_bootloader_mode() From f5744f8a3f5ddfe80152fdacc4f675f6ed451f35 Mon Sep 17 00:00:00 2001 From: Jan Brzyk Date: Thu, 23 Nov 2023 14:34:57 +0100 Subject: [PATCH 16/18] small firmware flashing fix Signed-off-by: Jan Brzyk --- flash-firmware.py | 1 + 1 file changed, 1 insertion(+) diff --git a/flash-firmware.py b/flash-firmware.py index 6b595ad..fc8c8b5 100755 --- a/flash-firmware.py +++ b/flash-firmware.py @@ -85,6 +85,7 @@ def flash_firmware(self): try: sh.stm32flash(self.port, "-v", w=self.binary_file, b="115200", _out=sys.stdout) time.sleep(0.2) + break except Exception: pass else: From 5ac9bfc196c9216ab7278432e2d2605ad367fd8e Mon Sep 17 00:00:00 2001 From: Jan Brzyk Date: Thu, 23 Nov 2023 15:56:19 +0100 Subject: [PATCH 17/18] firmware flashing fix Signed-off-by: Jan Brzyk --- flash-firmware.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/flash-firmware.py b/flash-firmware.py index fc8c8b5..89bb0eb 100755 --- a/flash-firmware.py +++ b/flash-firmware.py @@ -65,20 +65,24 @@ def flash_firmware(self): 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.") + print("WARNING! Disabling the flash Write-Protection went wrong.") # Disable the flash read-protection for i in range(self.max_approach_no): try: 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.") + print("WARNING! Disabling the flash Read-Protection went wrong.") # Flashing the firmware for i in range(self.max_approach_no): @@ -87,6 +91,7 @@ def flash_firmware(self): time.sleep(0.2) break except Exception: + print("Flashing error! Trying again.") pass else: print("ERROR! Flashing the firmware went wrong. Try again.") @@ -111,7 +116,7 @@ def main(): flasher = FirmwareFlasher(sys_arch, binary_file) flasher.flash_firmware() - print("Done.") + print("Done!") if __name__ == "__main__": From 57cd88c5d2d6ef7c11bb7b81bce9630bb44bee4d Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 27 Nov 2023 10:35:29 +0100 Subject: [PATCH 18/18] Fix Vulcannexus healthcheck --- Dockerfile.hardware | 10 ++++++++-- Dockerfile.simulation | 31 +++++++++++++------------------ 2 files changed, 21 insertions(+), 20 deletions(-) diff --git a/Dockerfile.hardware b/Dockerfile.hardware index 7aa6faf..d33fe6c 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -115,9 +115,15 @@ 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 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_node &' \ - /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_node &' \ + /vulcanexus_entrypoint.sh; \ + fi COPY ./healthcheck.sh / HEALTHCHECK --interval=7s --timeout=2s --start-period=5s --retries=5 \ diff --git a/Dockerfile.simulation b/Dockerfile.simulation index 5c1adb1..efcd365 100644 --- a/Dockerfile.simulation +++ b/Dockerfile.simulation @@ -18,26 +18,18 @@ 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 health check package + # 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 && \ @@ -54,10 +46,7 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 && \ @@ -65,9 +54,15 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ 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 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_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 &' \ - /ros_entrypoint.sh + /vulcanexus_entrypoint.sh; \ + fi COPY ./healthcheck.sh / HEALTHCHECK --interval=7s --timeout=2s --start-period=5s --retries=5 \