Changes in healthcheck

.devcontainer/Dockerfile

@@ -28,6 +28,7 @@ RUN mkdir -p /home/$USERNAME/ros2_ws/src && \
     echo 'source /home/'$USERNAME'/ros2_ws/install/setup.bash' >> /home/$USERNAME/.bashrc
 
 ENV TERM=xterm-256color
+ENV RCUTILS_COLORIZED_OUTPUT=1
 
 # Setup entrypoint
 COPY ./ros_entrypoint.sh /

rosbot/rosbot_hardware.repos

@@ -18,4 +18,4 @@ repositories:
   rosbot_hardware_interfaces:
     type: git
     url: https://github.com/husarion/rosbot_hardware_interfaces.git
-    version: 9abfb05b0ffad89935ce457de22c24a53cd728cb
+    version: 2ff82cd9bfcb9692ab606ad3ae57fe209ad49548

rosbot_bringup/launch/healthcheck.launch.py

@@ -13,83 +13,23 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
-import subprocess
-
 from launch import LaunchDescription
-from launch.actions import EmitEvent, OpaqueFunction, TimerAction
-from launch.events import Shutdown
-from launch.substitutions import LaunchConfiguration
+from launch.actions import OpaqueFunction, TimerAction
 
 
 def check_controller_status(context):
-    healthcheck = LaunchConfiguration("healthcheck").perform(context)
-    if healthcheck.lower() == "false":
-        with open("/var/tmp/rosbot_status.txt", "w") as status_file:
-            status_file.write("healthy")
-        return
-    else:
-        with open("/var/tmp/rosbot_status.txt", "w") as status_file:
-            status_file.write("unhealthy")
-
-    use_sim = LaunchConfiguration("use_sim", default="False").perform(context)
-    nodes_to_check = [
-        "/controller_manager",
-        "/ekf_filter_node",
-        "/imu_broadcaster",
-        "/joint_state_broadcaster",
-        "/laser_scan_box_filter",
-        "/robot_state_publisher",
-        "/rosbot_base_controller",
-        "/scan_to_scan_filter_chain",
-    ]
-    if use_sim.lower() == "true":
-        additional_nodes = [
-            "/gz_ros2_control",
-            "/ros_gz_bridge",
-        ]
-    else:
-        additional_nodes = [
-            "/imu_sensor_node",
-            # "/rosbot_ros2_firmware", not visible via USB port
-            "/rosbot_system_node",
-        ]
-    nodes_to_check.extend(additional_nodes)
-
-    namespace = LaunchConfiguration("namespace", default="").perform(context)
-    ns = "/" + namespace if namespace else ""
-    nodes_to_check = [ns + node for node in nodes_to_check]
-
-    def get_missing_nodes():
-        env = os.environ.copy()
-        env["ROS_LOCALHOST_ONLY"] = "1"
-
-        result = subprocess.run(
-            ["ros2", "node", "list"],
-            stdout=subprocess.PIPE,
-            text=True,
-            env=env,
-        )
-        active_nodes = result.stdout.splitlines()
-        return [node for node in nodes_to_check if node not in active_nodes]
-
-    missing_nodes = get_missing_nodes()
     green_color = "\033[92m"
-    red_color = "\033[91m"
     reset_color = "\033[0m"
 
-    if missing_nodes:
-        print(
-            f"{red_color}Error: Some nodes are not initialized: {missing_nodes}. Emitting shutdown...{reset_color}"
-        )
-        return [EmitEvent(event=Shutdown())]
-    else:
-        print(f"{green_color}All systems are up and running!{reset_color}")
-        with open("/var/tmp/rosbot_status.txt", "w") as status_file:
-            status_file.write("healthy")
+    print(f"{green_color}All systems are up and running!{reset_color}")
+    with open("/var/tmp/rosbot_status.txt", "w") as status_file:
+        status_file.write("healthy")
 
 
 def generate_launch_description():
+    with open("/var/tmp/rosbot_status.txt", "w") as status_file:
+        status_file.write("unhealthy")
+
     check_controller = TimerAction(
         period=15.0,
         actions=[OpaqueFunction(function=check_controller_status)],

rosbot_controller/launch/controller.launch.py

@@ -14,8 +14,15 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, TimerAction
+from launch.actions import (
+    DeclareLaunchArgument,
+    EmitEvent,
+    RegisterEventHandler,
+    TimerAction,
+)
 from launch.conditions import UnlessCondition
+from launch.event_handlers import OnProcessIO
+from launch.events import Shutdown
 from launch.substitutions import (
     Command,
     FindExecutable,
@@ -173,6 +180,29 @@ def generate_launch_description():
         ],
     )
 
+    def check_if_log_is_fatal(event):
+        if "fatal" in event.text.decode().lower():
+            return EmitEvent(event=Shutdown(reason=f"Fatal error: {event.text}"))
+
+    joint_state_monitor = RegisterEventHandler(
+        OnProcessIO(
+            target_action=joint_state_broadcaster_spawner,
+            on_stderr=lambda event: check_if_log_is_fatal(event),
+        )
+    )
+    robot_controller_monitor = RegisterEventHandler(
+        OnProcessIO(
+            target_action=robot_controller_spawner,
+            on_stderr=lambda event: check_if_log_is_fatal(event),
+        )
+    )
+    imu_broadcaster_monitor = RegisterEventHandler(
+        OnProcessIO(
+            target_action=imu_broadcaster_spawner,
+            on_stderr=lambda event: check_if_log_is_fatal(event),
+        )
+    )
+
     return LaunchDescription(
         [
             declare_namespace_arg,
@@ -181,5 +211,8 @@ def generate_launch_description():
             control_node,
             robot_state_pub_node,
             delayed_spawner_nodes,
+            joint_state_monitor,
+            robot_controller_monitor,
+            imu_broadcaster_monitor,
         ]
     )

rosbot_description/urdf/rosbot_macro.urdf.xacro

@@ -23,8 +23,8 @@
       <ros2_control name="${ns}imu" type="sensor">
         <hardware>
           <plugin>rosbot_hardware_interfaces/RosbotImuSensor</plugin>
-          <param name="connection_timeout_ms">120000</param>
-          <param name="connection_check_period_ms">500</param>
+          <param name="connection_timeout_ms">30000</param>
+          <param name="connection_check_period_ms">1000</param>
         </hardware>
         <sensor name="${ns}imu">
           <state_interface name="orientation.x" />
@@ -45,8 +45,8 @@
       <hardware>
         <xacro:unless value="$(arg use_sim)">
           <plugin>rosbot_hardware_interfaces/RosbotSystem</plugin>
-          <param name="connection_timeout_ms">120000</param>
-          <param name="connection_check_period_ms">500</param>
+          <param name="connection_timeout_ms">30000</param>
+          <param name="connection_check_period_ms">1000</param>
 
           <!-- Firmware from https://github.com/husarion/rosbot_ros2_firmware could not override this names -->
           <param name="velocity_command_joint_order">