1. Aquisição do código-fonte

O oficial não fornece a versão ROS2 do pacote de meta-função smach, podemos usar o pacote transplantado pelo DeepX:

Clone os dois pacotes acima no espaço de trabalho e, em seguida, rosdep + colcon build + source

2. Aquisição de rotina

Os tutoriais de amostra na Internet são todos ROS1, vamos mudar para a versão ROS2:

Primeiro cd para o diretório src para criar um novo pacote de recursos

ros2 pkg create --build-type ament_python --dependencies rclpy smach smach_ros

import rclpy
from rclpy.node import Node
import smach
import smach_ros

class SmachTestNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self.get_logger().info("启动 demo 节点")

# 定义状态 Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

    def execute(self, userdata):
        print('Executing state FOO')
        if self.counter < 3:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'

# 定义状态 Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome2'])

    def execute(self, userdata):
        print('Executing state BAR')
        return 'outcome2'

# main
def main(args=None):
    rclpy.init(args=args) # 初始化 ros
    node = SmachTestNode("execute_smach_test")

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])

    # Open the container
    with sm:
    # Add states to the container
        smach.StateMachine.add('FOO', Foo(), 
            transitions={
    
    'outcome1':'BAR', 'outcome2':'outcome4'})
        smach.StateMachine.add('BAR', Bar(), 
            transitions={
    
    'outcome2':'FOO'})

    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    outcome = sm.execute()

    # Wait for ctrl-c to stop the application
    rclpy.spin(node)
    sis.stop()

if __name__ == '__main__':
    main()

Depois de iniciar o nó é exibido

insira a descrição da imagem aqui

Digite em outro terminal ros2 run smach_viewer smach_viewer_gui.pypara visualizar a máquina de estado
insira a descrição da imagem aqui

3 análise de rotina

Existem quatro conceitos de máquina de estado.

Estado , estado. Uma máquina de estado deve conter pelo menos dois estados. Por exemplo, uma porta automática tem dois estados: aberta e fechada.
Evento , evento. Um evento é uma condição de acionamento ou senha para executar uma operação. Para portas automáticas, "pressione o botão de abertura da porta" é um evento.
Ação , ação. Ações a serem executadas após a ocorrência de um evento. Por exemplo, o evento é "pressione o botão para abrir a porta" e a ação é "abra a porta". Na programação, uma Ação geralmente corresponde a uma função.
Transição , transformação. Ou seja, mudar de um estado para outro. Por exemplo, "processo de abertura de porta" é uma transformação.

Como uma máquina de estados, primeiro ela precisa ter um estado, existem dois estados nessa rotina: FOO e BAR. Esses dois estados são definidos por funções do Python, e a estrutura é semelhante, incluindo as duas funções de inicialização ( init ) e execução (execute).

3.1 Função de inicialização de estado

A função de inicialização é usada para inicializar a classe de estado, chamar a função de inicialização do estado em smach e precisar definir o estado de saída: resultado1, resultado2

def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

O resultado aqui representa o valor de saída ao final do estado , representado por uma string, e a faixa de valores é definida pelo usuário. Por exemplo, podemos definir se a execução do estado foi bem-sucedida: ['succeeded', 'failed', 'awesome']. Cada estado pode ter vários valores de saída e é possível pular para um próximo estado diferente de acordo com diferentes saídas valores.

Observação: a função de inicialização não pode ser bloqueada.Se você precisar implementar funções de bloqueio, como sincronização, poderá usar a implementação multiencadeada.

3.2 Função de execução da ação

A função de execução é o conteúdo de trabalho específico em cada estado, e pode executar trabalho de bloqueio.Quando o trabalho precisa retornar o valor de saída definido, o estado termina.

def execute(self, userdata):
        print('Executing state FOO')
        if self.counter < 3:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'

3.3 função principal

Na função principal, primeiro inicialize o nó ROS

Em seguida, use StateMachine para criar uma máquina de estado e especifique dois valores de saída final após a execução da máquina de estado: resultado4 e resultado5.

# 创建一个 SMACH 状态机
    sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])

A máquina de estado SMACH é um contêiner . Podemos usar o método add para adicionar o estado necessário ao contêiner da máquina de estado e, ao mesmo tempo, precisamos definir a relação de salto entre os estados.

# 打开容器
    with sm:
    # 将状态添加到容器中
        smach.StateMachine.add('FOO', Foo(), 
            transitions={
    
    'outcome1':'BAR', 'outcome2':'outcome4'})
        smach.StateMachine.add('BAR', Bar(), 
            transitions={
    
    'outcome2':'FOO'})

Por exemplo, primeiro adicionamos um estado denominado "FOO" à máquina de estado. A classe desse estado é a Foo que definimos anteriormente, e as transições representam mudanças de estado (ou seja, o quarto conceito da máquina de estado). o estado é executado, resultado de saída1 , em seguida, salte para o estado "BAR", se o resultado de saída2 for executado, finalize esta máquina de estado e resultado de saída4.

Lembre-se da interface visual que vimos acima, para visualizar a máquina de estados, precisamos adicionar um servidor visual ao código:

# Create and start the introspection server
  sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
  sis.start()

O método IntrospectionServer() é usado para criar um servidor de visualização interno com três parâmetros:

O primeiro parâmetro é o nome do servidor, que pode ser fornecido livremente conforme a necessidade;
O segundo parâmetro é a máquina de estado a ser monitorada;
O terceiro parâmetro representa o nível da máquina de estado, porque a máquina de estado SMACH suporta aninhamento e o estado também pode ter sua própria máquina de estado .

Então você pode usar o método execute() para iniciar a execução da máquina de estado:

# Execute SMACH plan
    outcome = sm.execute()

Após a execução, o servidor de visualização interno precisa ser parado:

sis.stop()

4. Resumo

Agora vamos revisar toda a máquina de estado:
insira a descrição da imagem aqui

Podemos ver na figura que, depois que a máquina de estado começa a funcionar, ela primeiro pula para o primeiro estado "FOO" que adicionamos e, em seguida, acumula a variável do contador nesse estado. Quando o contador for menor que 3, ele produzirá resultado1, e pule depois que o estado terminar. Vá para o estado "BAR". No estado "BAR" nada é feito, a saída result2 retorna ao estado "FOO". Depois de ir e voltar várias vezes dessa maneira, o contador é igual a 3, o valor de saída do estado "FOO" torna-se resultado2 e, em seguida, salta para resultado4, o que significa que a máquina de estado finito está concluída . Outcome5 não está envolvido em todo o processo, tornando-se um nó isolado no grafo.

A máquina de estado acima pode ser imaginada como uma simples aplicação de robô: o robô pega o copo na mesa, termina a tarefa se o agarrar, continua tentando se não conseguir agarrá-lo e desiste se não conseguir pegá-lo após 3 tentativas Agarre, termine a tarefa.

Pós-escrito: Uma máquina de estado complexa: