ROS2, state transition not possible

[slyandsmart]

• Python State Machine version: 2.1.2
• Python version: 3.10
• Operating System: Ubuntu 22.04, ROS2 Humble

Description

I want to use the state-machine to control the robots state in a simple pic and place example. But i tried to do a minimal example just to switch between states by subscribing a ros2 topic with an string message.

I get an error while transision. statemachine.exceptions.TransitionNotAllowed: Can't StateA when in StateA.

I would expect to get an log output in on of the states. It should transition. while i subscibe the message.

MINIMAL EXAMPLE:
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from std_msgs.msg import String
from statemachine import StateMachine, State

class MyStateMachine(StateMachine):
    state_a = State('StateA', initial=True)
    state_b = State('StateB')
    state_c = State('StateC')
    trigger = state_a.to(state_b) | state_b.to(state_c) | state_c.to(state_a)

    def on_state_a(self):
        self.node.get_logger().info('Executing function for StateA')
    def on_state_b(self):
        self.node.get_logger().info('Executing function for StateB')
    def on_state_c(self):
        self.node.get_logger().info('Executing function for StateC')

class StateMachineNode(Node):
    def __init__(self):
        super().__init__('state_machine_node')
        self.my_fsm = MyStateMachine()
        self.subscription = self.create_subscription(
            String,
            'state_transition',
            self.state_transition_callback,
            10)

    def state_transition_callback(self, msg):
        new_state = msg.data
        if new_state in ['StateA', 'StateB', 'StateC']:
            self.my_fsm.send(new_state)
            self.get_logger().info(f'Transitioning to {new_state}')

def main(args=None):
    rclpy.init(args=args)
    node = StateMachineNode()
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    try:
        node.get_logger().info('State machine node running')
        executor.spin()
    except KeyboardInterrupt:
        node.get_logger().info('Shutting down')
        rclpy.shutdown()

if __name__ == '__main__':
    main()

TRACEBACK:
Traceback (most recent call last):
  File "/martinDock2Ros2/workspace/install/exu_ros2_interface/lib/exu_ros2_interface/exu_test", line 33, in <module>
    sys.exit(load_entry_point('exu-ros2-interface==0.0.0', 'console_scripts', 'exu_test')())
  File "/martinDock2Ros2/workspace/install/exu_ros2_interface/lib/python3.10/site-packages/exu_ros2_interface/exu_test.py", line 53, in main
    executor.spin()
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 294, in spin
    self.spin_once()
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 794, in spin_once
    self._spin_once_impl(timeout_sec)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 791, in _spin_once_impl
    future.result()
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 94, in result
    raise self.exception()
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 239, in __call__
    self._handler.send(None)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 437, in handler
    await call_coroutine(entity, arg)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 362, in _execute_subscription
    await await_or_execute(sub.callback, msg)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 107, in await_or_execute
    return callback(*args)
  File "/martinDock2Ros2/workspace/install/exu_ros2_interface/lib/python3.10/site-packages/exu_ros2_interface/exu_test.py", line 39, in state_transition_callback
    self.my_fsm.send(new_state)
  File "/home/vscode/.local/lib/python3.10/site-packages/statemachine/statemachine.py", line 359, in send
    return event.trigger(self, *args, **kwargs)
  File "/home/vscode/.local/lib/python3.10/site-packages/statemachine/event.py", line 29, in trigger
    return machine._process(trigger_wrapper)
  File "/home/vscode/.local/lib/python3.10/site-packages/statemachine/statemachine.py", line 288, in _process
    return self._processing_loop()
  File "/home/vscode/.local/lib/python3.10/site-packages/statemachine/statemachine.py", line 303, in _processing_loop
    result = trigger()
  File "/home/vscode/.local/lib/python3.10/site-packages/statemachine/event.py", line 27, in trigger_wrapper
    return self._trigger(trigger_data)
  File "/home/vscode/.local/lib/python3.10/site-packages/statemachine/event.py", line 44, in _trigger
    raise TransitionNotAllowed(trigger_data.event, state)
statemachine.exceptions.TransitionNotAllowed: Can't StateA when in StateA.

[slyandsmart]

I think i found a solution by myself how work within the python-statemachine in combination with ros2.

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from std_msgs.msg import String
from statemachine import StateMachine, State

class MyStateMachine(StateMachine):
    state_a = State(initial=True)
    state_b = State()
    state_c = State()
    trigger = state_a.to(state_b,cond='condition_success') |state_a.to(state_c,cond='condition_success2') | state_b.to(state_c) | state_c.to(state_a)

class StateMachineNode(Node):
    def __init__(self):
        super().__init__('state_machine_node')
        self.myfsm = MyStateMachine(self)
        self.subscription = self.create_subscription(
            String,
            'state_transition',
            self.state_transition_callback,
            10)

    def condition_success(self):
        return self.new_state == 'StateA'
    def condition_success2(self):
        return self.new_state == 'StateB'

    def state_transition_callback(self, msg):
        self.new_state = msg.data
        if self.new_state in ['StateA', 'StateB', 'StateC']:
            self.myfsm.send('trigger')
            self.get_logger().info(f'Transitioning trigger got it  {self.new_state}')

    def on_enter_state_a(self):
        self.get_logger().info('Executing function for StateA')

    def on_enter_state_b(self):
        self.get_logger().info('Executing function for StateB')

    def on_enter_state_c(self):
        self.get_logger().info('Executing function for StateC')

def main(args=None):
    rclpy.init(args=args)
    node = StateMachineNode()
    #fsm = MyStateMachine(node)
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    try:
        node.get_logger().info('State machine node running')
        executor.spin()

    except KeyboardInterrupt:
        node.get_logger().info('Shutting down')
        rclpy.shutdown()

if __name__ == '__main__':
    main()

This implementation allows me to use the trigger form a ros2 topic within the state machine. So my full functionality can be in the ros2 node.

[fgmacedo]

Nice @slyandsmart . After reading your solution the fix was to "send" to the state machine the event name, not the name of the target state. Right? In your case, the event name is trigger.

Changing this line;

self.my_fsm.send(new_state)

By this one:

self.myfsm.send('trigger')

Also, I think that you'll better served if the code surrounding the state machine don't know anything about the internal states.

So if you could refactor the event loop to receive the performed actions instead the desired state, your design will improve:

Something like this:

    def state_transition_callback(self, msg):
        event = msg.data
        if event in self.myfsm.allowed_events:
            self.myfsm.send(event)
            self.get_logger().info(f'Transitioning trigger got it  {event}. Current state is {self.myfsm.current_state.id}')

As you've resolved the problem, I'll close the issue for now. Feel free to reopen it or open another one.