Add healthcheck to simulation

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 '<version>' | sed -r 's/.*<version>([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') > /version.txt
+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
+
+HEALTHCHECK --interval=10s --timeout=5s --start-period=5s --retries=5 \
+    CMD ["/ros_entrypoint.sh", "ros2", "run", "healthcheck_pkg", "healthcheck_node"]

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}
+    command: ros2 launch rosbot_gazebo simulation.launch.py mecanum:=${MECANUM:-False}

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<nav_msgs::msg::Odometry>(TOPIC_NAME, rclcpp::SensorDataQoS(), msg_callback);
+  auto timer = node->create_wall_timer(TIMEOUT, timeout_callback);
+
+  rclcpp::spin(node);
+  return msg_received;
+}