Wiki
Clone wikicola2_core / safety
COLA2 Safety
cola2_safety is the package in charge of the vehicle’s safety. It contains several safety controllers and safety monitors that perform checks on some important values in the architecture. The main safety workflow is composed of three entities. First, the diagnostics system is in charge of reporting most of the data to monitor the correct status of different COLA2 components. These status data are summarized in a single message by the vehicle_status_parser
node and then checked by the safety_supervisor
node who is responsible for triggering certain actions when some of these values are not inside the user predefined limits. All the parameters used by the safety nodes are defined in the file cola2_<vehicle>/config/safety.yaml
.
Vehicle diagnostics - Vehicle status parser - Safety supervisor
The figure below summarizes the main safety workflow in COLA2.
-
Most of the COLA2 nodes publish messages of type
diagnostic_msgs/DiagnosticStatus
on the topic/<vehicle>/diagnostics
, with the node name, the status (OK, WARN, ERROR) and specific diagnostics data. The diagnostic messages can be published from different nodes: directly by the driver node that generates the information (e.g. the battery driver publishes the battery level), by a third node that uses this information (e.g. the navigator node publishes the last time that data from a navigation sensor has arrived), or by a safety node. -
All the diagnostic messages are combined at runtime using the diagnostic_aggregator node, which publishes a diagnostics_agg topic (remapped to /sparus2/diagnostics_agg). These combined diagnostics can be visualized in a categorized way using the rqt_robot_monitor tool in the following way:
rosrun rqt_robot_monitor rqt_robot_monitor diagnostics_agg:=/<vehicle>/diagnostics_agg
-
The
vehicle_status_parser
node is in charge of receiving the aggregated diagnostics message and parse it to fill up acola2_msgs/VehicleStatus
message. This message, published in the topic/<vehicle>/vehicle_status
, summarizes the current state of the vehicle and includes safety values collected from the diagnostics as well as other information from the status of the vehicle coming from the captain and the navigator. -
The
safety_supervisor
is the one in charge of detecting errors in the system based on the current vehicle status information. It subscribes to the/<vehicle>/vehicle_status
topic and applies a set of rules that compare the current status values against the configuration values set on thecola2_<vehicle>/config/safety.yaml
file. If any of the rule checks is triggered, the safety supervisor calls a recovery actions. -
The safety supervisor publishes on the topic
/<vehicle>/safety_supervisor/status
acola2_msgs/SafetySupervisorStatus
message that includes the currenterror_code
and the recovery action that is ongoing (in case there is any). -
The
error_code
is astd_msgs/Int16
message that codifies, bitwise, different status information, errors and warnings in two bytes. This error_code is used to pass information about the state of the vehicle in a compact way, for instance when using acoustic communication to know the state of the vehicle at surface.
Recovery Actions
Recovery actions are implemented in the recovery_actions
node. They can be automatically called by the safety_supervisor
node according to the rules explained above or manually using the following service:
rosservice call /<vehicle>/recovery_actions/recover requested_ra
Where requested_ra
must be a message of type cola2_msgs/RecoveryAction
that essentially has an error_level
and an error_message
to describe what has triggered the action.
The error_level
identifies the type of recovery action, which can be:
- INFORMATIVE: Informs that an error is produced.
- ABORT_MISSION: INFORMATIVE + aborts goto, submerge, keep position and mission play services.
- ABORT_AND_SURFACE: ABORT_MISSION + brings the vehicle to a safety depth.
- EMERGENCY_SURFACE: ABORT_MISSION + without using the navigation or the control stack sends a setpoint directly to the thrusters to make the vehicle surface. After an emergency surface COLA2 has to be restarted.
The following parameters of the safety.yaml
file must be set for the recovery action service to work properly:
#Depth to bring the vehicle on ABORT_AND_SURFACE recovery_actions/controlled_surface_depth: 0.0 #Thruster setpoints used to perform EMERGENCY_SURFACE [thruster 0,1,2] recovery_actions/emergency_surface_setpoints: [-0.30, 0.0, 0.0]
Safety controllers
There are two nodes that can actively control the vehicle for safety purposes.
safe_depth_altitude
This node prevents the vehicle to go behind a maximum depth or a minimum altitude. These two limit values are configured using the parameters safe_depth_altitude/max_depth
and safe_depth_altitude/min_altitude
. When reaching the specified limits, this controller generates a BodyVelocityReq
message asking the vehicle a negative heave with maximum priority (overriding manual operation).
set_zero_velocity
Once the vehicle is below a predefined depth, this node starts publishing BodyVelocityReq
messages to keep the vehicle velocities at 0 and thus avoid the inertia that the vehicle has when a particular request is stopped. To simplify the combination of several DoF by the control stack, instead of publishing a single message requesting a 0.0 velocity for all DoFs, several messages are published for each DOF.
safety_set_zero_velocity/set_zero_velocity_depth: 1.0 safety_set_zero_velocity/set_zero_velocity_axis:[[False, True, True, True, True, True], [True, True, False, True, True, True], [True, True, True, True, True, False]]
Using this configuration, the vehicle will publish three BodyVelocityReq
messages once the
vehicle depth is greater than 1 meter. The first message will request Surge = 0, the second
Heave = 0, while the last one Yaw = 0.
Safety value monitors
COLA2 Watchdog ###
This node keeps track of the cola2 running time. Counts the elapsed time since the architecture started or since the last time reset, which can be done through a provided service (/<vehicle>/cola2_watchdog/reset_timeout
). Publishes the current elapsed time.
Virtual cage
This node alerts (with an Informative action) if the vehicle goes out of a predefined area, defined by a NED origin (virtual_cage/north_origin
and virtual_cage/east_origin
) and some extents for the north and east axes (virtual_cage/north_longitude
and virtual_cage/east_longitude
).
Updated