Ros2 healthcheck fix (#10)

* healthcheck fix
* QoS fix

husarion_utils/healthcheck.cpp

@@ -13,8 +13,13 @@ class HealthCheckNode : public rclcpp::Node {
       : Node("healthcheck_astra"),
         last_msg_time(std::chrono::steady_clock::now()) {
 
+    // Set custom QoS settings to match the publisher
+    auto qos = rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10));
+    qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
+
     subscription_ = create_subscription<sensor_msgs::msg::LaserScan>(
-        "scan", rclcpp::SensorDataQoS().keep_last(1),
+        "scan", qos,
         std::bind(&HealthCheckNode::msgCallback, this, std::placeholders::_1));
 
     timer_ = create_wall_timer(HEALTHCHECK_PERIOD,
@@ -37,9 +42,8 @@ class HealthCheckNode : public rclcpp::Node {
   }
 
   void healthyCheck() {
-    std::chrono::steady_clock::time_point current_time =
-        std::chrono::steady_clock::now();
-    std::chrono::duration<double> elapsed_time = current_time - last_msg_time;
+    auto current_time = std::chrono::steady_clock::now();
+    auto elapsed_time = current_time - last_msg_time;
     bool is_msg_valid = elapsed_time < MSG_VALID_TIME;
 
     if (is_msg_valid) {
@@ -52,7 +56,8 @@ class HealthCheckNode : public rclcpp::Node {
 
 int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<HealthCheckNode>());
+  auto node = std::make_shared<HealthCheckNode>();
+  rclcpp::spin(node);
   rclcpp::shutdown();
   return 0;
 }

husarion_utils/rplidar.launch.py

@@ -1,15 +1,21 @@
+import os
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
     IncludeLaunchDescription,
     GroupAction,
     OpaqueFunction,
+    ExecuteProcess,
 )
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node, PushRosNamespace, SetRemap
 from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
 
+def create_health_status_file():
+    with open("/var/tmp/health_status.txt", "w") as file:
+        file.write("healthy")
 
 def launch_setup(context, *args, **kwargs):
     launch_file = LaunchConfiguration("launch_file").perform(context)
@@ -48,13 +54,21 @@ def launch_setup(context, *args, **kwargs):
 
     rplidar_ns = GroupAction(actions=rplidar_actions)
 
-    # Health check
+    # Retrieve the healthcheck argument
+    healthcheck = LaunchConfiguration("healthcheck").perform(context)
+
+    # Conditional file creation based on the healthcheck argument
+    if healthcheck == 'False':
+        create_health_status_file()
+
+    # Define the healthcheck node
     healthcheck_node = Node(
         package="healthcheck_pkg",
         executable="healthcheck_node",
         name="healthcheck_rplidar",
         namespace=device_namespace,
         output="screen",
+        condition=IfCondition(healthcheck)
     )
 
     return [PushRosNamespace(robot_namespace), rplidar_ns, healthcheck_node]
@@ -88,6 +102,11 @@ def generate_launch_description():
                 default_value="",
                 description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
             ),
+            DeclareLaunchArgument(
+                "healthcheck",
+                default_value="False",
+                description="Enable health check for RPLIDAR.",
+            ),
             OpaqueFunction(function=launch_setup),
         ]
     )