1. Iqua Robotics Avatar Iqua Robotics
  2. cola2
  3. cola2_core

Wiki

Clone wiki

cola2_core / safety

View History

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 a cola2_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 the cola2_<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 a cola2_msgs/SafetySupervisorStatus message that includes the current error_code and the recovery action that is ongoing (in case there is any).

  • The error_code is a std_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