micro-ROS / system_modes

System modes for ROS 2 and micro-ROS
Apache License 2.0
43 stars 9 forks source link

Problem changing the system modes when there are rules #94

Open fmrico opened 2 years ago

fmrico commented 2 years ago

Hi all

I am trying to use system modes for a system composed of these nodes: {nav2, manipulator, camera, guard}. guard is a special node that detects if the system has been compromised. In this case, all the nodes should be in mode COMPROMISED and camera in the inactive state. The specification is:

# system modes safety benchmark
---

safety:
  ros__parameters:
    type: system
    parts:
      manipulator
      nav2
      camera
      guard
    modes:
      __DEFAULT__:
        manipulator: active.__DEFAULT__
        nav2: active.__DEFAULT__
        camera: active.__DEFAULT__
        guard: active.__DEFAULT__
      COMPROMISED:
        manipulator: active.COMPROMISED
        nav2: active.COMPROMISED
        camera: inactive.COMPROMISED
        guard: active.COMPROMISED
    rules:
      guard_detect_compomise_in_default:
        if_target: active.__DEFAULT__
        if_part: [guard, active.COMPROMISED]
        new_target: active.COMPROMISED
      guard_detect_default_in_compromise:
        if_target: active.COMPROMISED
        if_part: [guard, active.__DEFAULT__]
        new_target: active.__DEFAULT__

manipulator:
  ros__parameters:
    type: node
    modes:
      __DEFAULT__:
        ros__parameters:
          max_torque: 0.5
      COMPROMISED:
        ros__parameters:
          max_torque: 0.1

nav2:
  ros__parameters:
    type: node
    modes:
      __DEFAULT__:
        ros__parameters:
          forbidden_areas: false
      COMPROMISED:
        ros__parameters:
          forbidden_areas: true

guard:
  ros__parameters:
    type: node
    modes:
      __DEFAULT__:
        ros__parameters:
          compromised_system: false
      COMPROMISED:
        ros__parameters:
          compromised_system: true

camera:
  ros__parameters:
    type: node
    modes:
      __DEFAULT__:
        ros__parameters:
          use_sim_time: false
      COMPROMISED:
        ros__parameters:
          use_sim_time: false

If I change the mode of guard, all the system reacts as I want. BUT if I want to command all the system to go from COMPROMISED to __DEFAULT__ I can't do it because guard is COMPROMISED, and that doesn't let the system reconfigure to __DEFAULT__.

Is there any way to combine the bottom-up rules with the possibility of changing the system mode (an up-bottom reconfiguration)?

Thanks!!