--- # Test State Machine
- READ_HEXAPOD_CURRENT_POSE:
    template: ReadTransformState
    target_frame: ur10_1/base
    source_frame: hexapod_1/top
    transform_output: hexapod_current_pose
    succeeded: MOVE_ABOVE_HEXAPOD_1

- MOVE_ABOVE_HEXAPOD_1:
    template: CartTrapVelActionState
    robot: ur10_1
    pose_input: hexapod_current_pose
    position_offset_x: 0.0
    position_offset_y: 0.0
    position_offset_z: -0.20
    rotation_offset_x: 0.0
    rotation_offset_y: 0.0
    rotation_offset_z: 0.0
    desired_velocity: 0.1
    succeeded: OPEN_TOOL_EXCHANGE_1

--- # Test State Machine
name: SM_TOP
template: StateMachineBase
node_name: test_simple_action_state_machine
states:
  - RECONFIGURE_HEXAPOD:
      template: StateMachine
      states:
      - READ_HEXAPOD_CURRENT_POSE:
          template: ReadTransformState
          target_frame: ur10_1/base
          source_frame: hexapod_1/top
          transform_output: hexapod_current_pose
          succeeded: MOVE_ABOVE_HEXAPOD_1
      
      - MOVE_ABOVE_HEXAPOD_1:
          template: CartTrapVelActionState
          robot: ur10_1
          pose_input: hexapod_current_pose
          position_offset_x: 0.0
          position_offset_y: 0.0
          position_offset_z: -0.20
          rotation_offset_x: 0.0
          rotation_offset_y: 0.0
          rotation_offset_z: 0.0
          desired_velocity: 0.1
          succeeded: OPEN_TOOL_EXCHANGE_1
      
      - OPEN_TOOL_EXCHANGE_1:
          template: SetOutputState
          robot: ur10_1
          output_number: TOOL_EXCHANGE_GPIO
          value: TOOL_EXCHANGE_OPEN
          succeeded: COUPLE_WITH_HEXAPOD 
  
      succeeded: SECONDARY_META_STATE
  
  - SECONDARY_META_STATE:
      template: StateMachine
      states:
      - SUB_STATE_1:
          template: ReadTransformState
          target_frame: ur10_2/base
          source_frame: hexapod_1/top
          transform_output: hexapod_current_pose
          succeeded: SUB_STATE_2
      
      - SUB_STATE_2:
          template: ReadTransformState
          target_frame: ur10_2/base
          source_frame: hexapod_1/top
          transform_output: hexapod_current_pose
          succeeded: TERNARY_META_STATE

      - TERNARY_META_STATE:
          template: StateMachine
          states:
          - SUB_SUB_STATE_1:
              template: ReadTransformState
              target_frame: ur10_2/base
              source_frame: hexapod_1/top
              transform_output: hexapod_current_pose
              succeeded: succeeded
  
      succeeded: succeeded

Replace old smacha test script with new one
Replace old smacha test script with new one
