Robot monitoring system in ROS 2 (0-20 points)

Goal

The goal of this task is to create a monitoring system for the robot. In general, the monitoring system should monitor the states of all robot’s components. If any issues are detected with a component, the system should promptly report the error and attempt to initiate recovery.

Description

Create the ROS 2 system that represents the robot state monitoring system. It should be divided into multiple nodes and each node should have just one responsibility.
The systems should contain the following nodes:

the state monitor,
the error handler,
and the sensor (lidar, camera) nodes.

The structure of the system has been shown on the below diagram.

The state monitor

The first node is a state monitor. It should check the state of each sensor by subscribing to the topics with names /[sensor_name]_state (Bool message type).
A list of monitored sensors should be defined in the configuration file.
The node expects that it receives a True state message from each sensor at least once in the specified time period (parameter). If not it generates the error state.
The error state could be also generated if the node receives False in the message from any sensor.
If the error state in any sensor is noticed, the state monitor should notify the ErrorHandler (with proper service that contains the sensor name) that specified sensor has an error.
It should also publish the /robot_state (Bool) topic depending on the current state of the whole system.

Error handler

It should restart the specified sensor (with service mechanism) if receives such request from the state monitor.
The ErrorHandler should implement the custom service server (service server example).

Sensor (e.g. lidar, camera)

Each sensor node should publish its state with specified frequency on the /[sensor_name]_state (Bool) topic.
It should implement the Service server to handle the restart of the device (service server example).

Remarks:

Create a custom service that will be used to notify the ErrorHandler.
Create a custom service that will be used to trigger the sensor restart and checking the restart procedure progress.
Parametrize nodes. All parameters should be placed in YAML files.
Create a ROS2 package and place all files inside the package.
Create a launcher (Python based launcher) that will start the state monitor and error handler nodes and load parameters from a files (the sensor nodes may be started separately). The system should be started based with single command
```
ros2 launch package_name launcher_name
```
Document your code!

Robotic Programming Environments: L7-L8