Hey all,

I implemented a simple state machine using smach (migrated to ROS2), that includes moveit action server in one of the states. The state machine, after succeeding, was not terminating, so I added a

node.destroy_node()
rclpy.shutdown()

at the end of the state machine. This resulted in terminating the node, but giving me this exception:

Exception in thread Thread-1:
Traceback (most recent call last):
  File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
    self.run()
  File "/usr/lib/python3.8/threading.py", line 870, in run
    self._target(*self._args, **self._kwargs)
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 290, in spin
    self.spin_once()
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 712, in spin_once
    handler, entity, node = self.wait_for_ready_callbacks(timeout_sec=timeout_sec)
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 698, in wait_for_ready_callbacks
    return next(self._cb_iter)
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 599, in _wait_for_ready_callbacks
    raise ExternalShutdownException()
rclpy.executors.ExternalShutdownException

I am not very familiar with threads, so I was wondering whether someone could explain me how I could track down this error.

The code for my state machine is:

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

from ccbts_api_msgs.action import Move
from ccbts_api_msgs.action import Grasp

from action_msgs.msg import GoalStatus
from rclpy.action import ActionClient

from rclpy.parameter import Parameter

# Webots
from std_msgs.msg import Bool

# Pymoveit2
from pymoveit2 import MoveIt2
from pymoveit2.robots import universal_robot
from rclpy.callback_groups import ReentrantCallbackGroup
from threading import Thread


class SendGoal(smach.State):  #Create state SendGoal
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['succeeded','failed'],
                             input_keys=['move_goal_in'])
        self.goal_msg = Move.Goal()
        self.node_ = rclpy.create_node('moveToPose_action_client')
        self.node_.get_logger().info('Created Webots move node')

        self.callback_group = ReentrantCallbackGroup()

        self.wait_rate = self.node_.create_rate(1.0)


    def execute(self, userdata):

        # Create MoveIt 2 interface
        self.moveit2 = MoveIt2(
            node=self.node_,
            joint_names=universal_robot.joint_names(),
            base_link_name=universal_robot.base_link_name(),
            end_effector_name=universal_robot.end_effector_name(),
            group_name=universal_robot.MOVE_GROUP_ARM,
            callback_group=self.callback_group,
        )
        # Spin the node in background thread(s)
        self.executor = rclpy.executors.MultiThreadedExecutor(2)
        self.executor.add_node(self.node_)
        self.executor_thread = Thread(target=self.executor.spin, daemon=True, args=())
        self.executor_thread.start()

        # Get parameters
        position = userdata.move_goal_in
        quat_xyzw = [0.70710678, 0.70710678, 0.0, 0.0]
        cartesian = False
        self.node_.get_logger().info(
                f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
            )
        self.moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw, cartesian=cartesian)

        while not self.moveit2.done_moving:
            self.wait_rate.sleep()
            self.node_.get_logger().info("Waiting")


        return 'succeeded'



class Grip(smach.State):  #Create state Grip
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['succeeded','failed'])

        self.msg = Bool()
        self.grasp_node = rclpy.create_node('grasp_command')
        self.grasp_node.get_logger().info('Created grasping node')
        self.publisher = self.grasp_node.create_publisher(Bool, 'vacuum_pump_center', 10)

    def execute(self, userdata):
        self.msg.data = True
        self.publisher.publish(self.msg)
        self.grasp_node.get_logger().info('Grasping' )
        self.grasp_node.destroy_node()
        return 'succeeded'


def pick_sm(x = sys.argv[2], y = sys.argv[3], z = sys.argv[4]): 

    rclpy.init(args=None)
    node = rclpy.create_node('pick')

    sm = smach.StateMachine(outcomes=['succeeded','failed'])
    sm.userdata.move_goal = [x, y, z]   #Set all the goals you want to sent to the robot.
    with sm:
        smach.StateMachine.add('SendGoal', SendGoal(),
                               transitions={'succeeded':'Grip',
                                            'failed':'failed',},
                               remapping={'move_goal_in':'move_goal'})
        smach.StateMachine.add('Grip', Grip(),
                               transitions={'succeeded':'succeeded',
                                            'failed':'failed',})

    sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
    sis.start()
    sm.execute()
    sis.stop()

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    pick_sm()

How can I terminate the node without having a threading error? Any input is appreciated. Thank you in advance.

PS: I used ROS2 foxy, smach, moveit2, pymoveit2 (a python moveit action server)

self.executor = rclpy.executors.MultiThreadedExecutor()
self.executor.add_node(self.grasp_node)
self.executor.spin_once(self.grasp_node)

self.__node.create_subscription(Bool, 'vacuum_pump_center', self.__control_center_vacuum_pump, 1, callback_group=ReentrantCallbackGroup())