355 navigation loss of gps signal or weak gps signal

.devcontainer/dev/Dockerfile

@@ -23,6 +23,8 @@ RUN apt update && apt install -y --no-install-recommends \
     ros-$ROS_DISTRO-moveit-visual-tools \
     ros-$ROS_DISTRO-moveit-ros-perception \
     ros-$ROS_DISTRO-topic-tools \
+    ros-$ROS_DISTRO-navigation2 \
+    ros-$ROS_DISTRO-nav2-bringup \
     && rm -rf /var/lib/apt/lists/* \
     && apt autoremove -y \
     && apt clean
@@ -46,9 +48,11 @@ RUN echo "source /.dev_bashrc" >> /home/$REMOTE_USER/.bashrc
 
 # Copy repo packages:
 COPY alliander_core/src/ /alliander/ros/src
+COPY alliander_diagnostics/src/ /alliander/ros/src
 COPY alliander_franka/src/ /alliander/ros/src
 COPY alliander_joystick/src/ /alliander/ros/src
 COPY alliander_moveit/src/ /alliander/ros/src
+COPY alliander_nav2/src/ /alliander/ros/src
 
 # Build repo packages:
 RUN /alliander/colcon_build.sh

alliander_core/alliander_core.Dockerfile

@@ -51,6 +51,7 @@ RUN apt update && apt install -y --no-install-recommends \
   ros-$ROS_DISTRO-vision-msgs \
   ros-$ROS_DISTRO-vision-msgs \
   ros-$ROS_DISTRO-geographic-msgs \
+  ros-$ROS_DISTRO-topic-tools \
   && rm -rf /var/lib/apt/lists/* \
   && apt autoremove -y \
   && apt clean

alliander_core/src/alliander_utilities/alliander_utilities/adapted_yaml.py

@@ -34,12 +34,12 @@ def get_ros_params(source_file: str) -> tuple[list[str], dict]:
         Args:
             source_file (str): Path to the YAML file.
 
-        Raises:
-            KeyError: If no or multiple 'ros__parameters' keys are found.
-
         Returns:
             tuple[list[str], dict]: A tuple containing the namespace as a list of strings and the
             'ros__parameters' dictionary.
+
+        Raises:
+            KeyError: If no or multiple 'ros__parameters' keys are found.
         """
         params = get_yaml(source_file)
         sub_dicts: list[tuple[list[str], dict]] = [([], params)]

alliander_core/src/alliander_utilities/alliander_utilities/launch_argument.py

@@ -61,11 +61,11 @@ def bool_value(self, context: LaunchContext) -> bool:
         Args:
             context (LaunchContext): The launch context in which to evaluate the argument.
 
-        Raises:
-            TypeError: If the string value cannot be interpreted as a boolean.
-
         Returns:
             bool: The boolean value of the launch argument.
+
+        Raises:
+            TypeError: If the string value cannot be interpreted as a boolean.
         """
         string_value = self.string_value(context)
         if string_value in {"True", "true"}:
@@ -93,11 +93,11 @@ def float_value(self, context: LaunchContext) -> float:
         Args:
             context (LaunchContext): The launch context in which to evaluate the argument.
 
-        Raises:
-            RuntimeError: If the float value is outside the specified min or max range.
-
         Returns:
             float: The float value of the launch argument.
+
+        Raises:
+            RuntimeError: If the float value is outside the specified min or max range.
         """
         string_value = self.string_value(context)
         float_value = float(string_value)

alliander_diagnostics/alliander_diagnostics.Dockerfile

@@ -0,0 +1,25 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+ARG BASE_IMAGE=ubuntu:latest
+FROM $BASE_IMAGE 
+
+ARG COLCON_BUILD_SEQUENTIAL
+ENV ROS_DISTRO=jazzy
+
+# Install repo packages:
+WORKDIR /$WORKDIR/ros
+COPY alliander_core/src/ /$WORKDIR/ros/src
+COPY alliander_diagnostics/src/ /$WORKDIR/ros/src
+RUN /$WORKDIR/colcon_build.sh
+
+# Install python dependencies:
+WORKDIR $WORKDIR
+COPY pyproject.toml /$WORKDIR/pyproject.toml
+RUN uv sync \
+  && echo "export PYTHONPATH=\"$(dirname $(dirname $(uv python find)))/lib/python3.12/site-packages:\$PYTHONPATH\"" >> /root/.bashrc \
+  && echo "export PATH=\"$(dirname $(dirname $(uv python find)))/bin:\$PATH\"" >> /root/.bashrc
+
+WORKDIR /$WORKDIR
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["sleep", "infinity"]

alliander_diagnostics/docker-compose.yml

@@ -0,0 +1,17 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+services:
+  alliander_diagnostics:
+    image: allianderrobotics/diagnostics
+    container_name: alliander_diagnostics
+    network_mode: host
+    privileged: true
+    mem_limit: 6gb
+    tty: true
+    volumes:
+      - "/tmp/.X11-unix:/tmp/.X11-unix"
+      - "/dev:/dev"
+    env_file:
+      - .env
+    command: ["/bin/bash", "-c", "ros2 launch alliander_diagnostics diagnostics.launch.py"]

alliander_diagnostics/src/alliander_diagnostics/CMakeLists.txt

@@ -0,0 +1,48 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.5)
+project(alliander_diagnostics)
+
+# CMake dependencies:
+find_package(ament_cmake REQUIRED)
+
+# Other dependencies:
+find_package(rclcpp REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# C++ executables:
+include_directories(include)
+add_executable(diagnostics
+  src/diagnostics_node.cpp
+  src/gps_diagnostics.cpp
+)
+target_include_directories(diagnostics PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(diagnostics
+  diagnostic_msgs
+  rclcpp
+  sensor_msgs
+)
+
+install(
+  TARGETS diagnostics
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Shared folders:
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Default test:
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

alliander_diagnostics/src/alliander_diagnostics/include/base_diagnostics.hpp

@@ -0,0 +1,57 @@
+// # SPDX-FileCopyrightText: Alliander N. V.
+//
+// # SPDX-License-Identifier: Apache-2.0
+
+#ifndef BASE_DIAGNOSTICS_HPP_
+#define BASE_DIAGNOSTICS_HPP_
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+
+/**
+ * @brief Base class for diagnostic modules.
+ *
+ * This class provides a common interface for evaluating and reporting
+ * diagnostic status of a component. Derived classes implement the
+ * evaluate() function to update the diagnostic status based on
+ * monitored data.
+ */
+class BaseDiagnostics {
+ public:
+  /**
+   * @brief Construct a BaseDiagnostics instance.
+   * @param name Name of the diagnostic component.
+   * @param hardware_id Identifier of the associated hardware.
+   */
+  BaseDiagnostics(const std::string& name, const std::string& hardware_id) {
+    status_.name = name;
+    status_.hardware_id = hardware_id;
+  }
+
+  virtual ~BaseDiagnostics() = default;
+
+  /**
+   * @brief Evaluate the monitored data and update the diagnostic status.
+   * @param now current time.
+   */
+  virtual void evaluate(rclcpp::Time now) = 0;
+
+  /**
+   * @brief Get the current diagnostic status.
+   * @return Copy of the diagnostic status message.
+   */
+  diagnostic_msgs::msg::DiagnosticStatus get_status() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return status_;
+  }
+
+ protected:
+  /// Current diagnostic status message.
+  diagnostic_msgs::msg::DiagnosticStatus status_;
+
+  /// Mutex protecting access to the status message.
+  std::mutex mutex_;
+};
+
+#endif  // BASE_DIAGNOSTICS_HPP_

alliander_diagnostics/src/alliander_diagnostics/include/diagnostics_node.hpp

@@ -0,0 +1,45 @@
+// # SPDX-FileCopyrightText: Alliander N. V.
+//
+// # SPDX-License-Identifier: Apache-2.0
+
+#ifndef DIAGNOSTICS_NODE_HPP_
+#define DIAGNOSTICS_NODE_HPP_
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "base_diagnostics.hpp"
+
+/**
+ * @brief Node responsible for collecting and publishing diagnostics.
+ *
+ * This class manages a set of diagnostic modules derived from
+ * BaseDiagnostics. It periodically evaluates each module and
+ * publishes the combined diagnostic status as a DiagnosticArray.
+ */
+class DiagnosticsNode {
+ public:
+  /**
+   * @brief Construct the diagnostics node.
+   * @param node The ROS2 node.
+   */
+  DiagnosticsNode(rclcpp::Node::SharedPtr node);
+
+ private:
+  /// ROS2 node used for logging, parameters and communication.
+  rclcpp::Node::SharedPtr node_;
+  /// Publisher for the aggregated diagnostics message.
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr pub_diag;
+  /// Timer used to periodically trigger diagnostics evaluation.
+  rclcpp::TimerBase::SharedPtr timer;
+  /// Collection of diagnostic modules managed by this node.
+  std::vector<std::shared_ptr<BaseDiagnostics>> diagnostics_modules;
+
+  /**
+   * @brief Timer callback that evaluates all diagnostic modules
+   * and publishes the resulting diagnostic array.
+   */
+  void diagnostics_cb();
+};
+
+#endif  // DIAGNOSTICS_NODE_HPP_

alliander_diagnostics/src/alliander_diagnostics/include/gps_diagnostics.hpp

@@ -0,0 +1,75 @@
+// # SPDX-FileCopyrightText: Alliander N. V.
+//
+// # SPDX-License-Identifier: Apache-2.0
+
+#ifndef GPS_DIAGNOSTICS_HPP_
+#define GPS_DIAGNOSTICS_HPP_
+
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+
+#include "base_diagnostics.hpp"
+
+/**
+ * @brief Diagnostic module for monitoring GPS health.
+ *
+ * This class subscribes to a NavSatFix topic and evaluates the
+ * quality and availability of the GPS signal. The diagnostic status
+ * is updated based on message reception, covariance values, and
+ * fix status.
+ */
+class GpsDiagnostics : public BaseDiagnostics {
+ public:
+  /**
+   * @brief Construct a GPSDiagnostics instance.
+   * @param node The ROS2 node to attach to.
+   * @param topic The topic of the GPS to monitor.
+   */
+  GpsDiagnostics(rclcpp::Node::SharedPtr node, const std::string& topic);
+
+ private:
+  /// The ROS2 node
+  rclcpp::Node::SharedPtr node_;
+
+  /// Subsciber for the GPS topic
+  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr sub_gps;
+
+  /// Indication of whether a GPS message is received (true) or not (false)
+  bool gps_msg_received = false;
+
+  /// Header time in seconds from latest received GPS data
+  rclcpp::Time latest_msg_time;
+  /// Covariance value from latest received GPS data
+  double latest_covariance = 0.0;
+  /// Fix status from latest received GPS data
+  int latest_fix_status = -1;
+
+  /// The number of seconds of having received no data until the GPS signal is
+  /// deemed to be unstable
+  double no_data_received_limit = 5.0;
+
+  /// Indication of whether a high covariance is detected (true) or not (false)
+  bool high_covariance_detected = false;
+  /// Start time of the detected high covariance value
+  rclcpp::Time high_covariance_start_time;
+
+  /// The limit of the GPS' covariance value, where a higher value indicates a
+  /// weak signal
+  double gps_covariance_limit = 30.0;
+  /// The number of seconds of ERROR status until the GPS signal is officially
+  /// deemed to be unstable
+  double gps_signal_instability_limit = 5.0;
+
+  /**
+   * @brief Monitor the GPS topic and save the data received.
+   * @param msg GPS fix message.
+   */
+  void gps_cb(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
+
+  /**
+   * @brief Evaluate the monitored data and update the diagnostic status.
+   * @param now current time.
+   */
+  void evaluate(rclcpp::Time now) override;
+};
+
+#endif  // GPS_DIAGNOSTICS_HPP_