smach on ros2 foxy not working

asked 2021-06-01 10:58:32 -0500

dinesh gravatar image

updated 2021-06-01 10:59:17 -0500

import rclpy

from rclpy.action import ActionServer
from rclpy.node import Node

from action_tutorials_interfaces.action import Fibonacci

import smach
import smach_ros


class FibonacciActionServer():

    def __init__(self, node):
        self._action_server = ActionServer(
            node,
            Fibonacci,
            'fibonacci',
            self.execute_callback)

    def execute_callback(self, msg):
        if msg.order == 0:
            self._sas.set_succeeded()
        elif msg.order == 1:
            self._sas.set_aborted()
        elif msg.order == 2:
            self._sas.set_preempted()

def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('simple_action_test')

    fibonacci_action_server = FibonacciActionServer(node)

    # Create a SMACH state machine
    sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])

    # Open the container
    with sm0:

        # Add a simple action state. This will use an empty, default goal
        # As seen in TestServer above, an empty goal will always return with
        # GoalStatus.SUCCEEDED, causing this simple action state to return
        # the outcome 'succeeded'
        smach.StateMachine.add('GOAL_DEFAULT',
                               smach_ros.SimpleActionState(node, 'fibonacci', Fibonacci),
                               {'succeeded':'GOAL_STATIC'})

        # Add another simple action state. This will give a goal
        # that should abort the action state when it is received, so we
        # map 'aborted' for this state onto 'succeeded' for the state machine.
        smach.StateMachine.add('GOAL_STATIC',
                               smach_ros.SimpleActionState(node, 'fibonacci', Fibonacci,
                                                       goal = Fibonacci.Goal(order=1)),
                               {'aborted':'GOAL_CB'})

        def goal_callback(userdata, default_goal):
            goal = Fibonacci.Goal()
            goal.order = 2
            return goal

        smach.StateMachine.add('GOAL_CB',
                               smach_ros.SimpleActionState(node, 'fibonacci', Fibonacci,
                                                       goal_cb = goal_callback),
                               {'aborted':'succeeded'})

        # For more examples on how to set goals and process results, see 
        # executive_smach/smach_ros/tests/smach_actionlib.py

    # Execute SMACH plan
    outcome = sm0.execute()

    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

When i run this this output comes and the process hangs. No idea why. Can anyone suggest me what should i do next?

[INFO] [1622562853.125051471] [smach_ros]: State machine starting in initial state 'GOAL_DEFAULT' with userdata: 
    []
edit retag flag offensive close merge delete