Robotics StackExchange | Archived questions

smach on ros2 foxy not working

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: 
    []

Asked by dinesh on 2021-06-01 10:58:32 UTC

Comments

Answers

smach package isn't supported for the distro foxy. To use smach, the distro would have to be noetic or melodic. You can check what packages are supported for any distro on ROS Index.

Asked by qilin_gundamenjoyer on 2022-06-11 23:18:08 UTC

Comments