ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

smach on ros2 foxy not working

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

dinesh gravatar image

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

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

1 Answer

Sort by ยป oldest newest most voted
2

answered 2022-06-11 23:18:08 -0600

qilin_gundamenjoyer gravatar image

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.

edit flag offensive delete link more

Question Tools

3 followers

Stats

Asked: 2021-06-01 10:58:32 -0600

Seen: 259 times

Last updated: Jun 11 '22