ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

ExternalShutdownException in thread

asked 2022-10-07 07:13:35 -0600

AlexandrosNic gravatar image

updated 2022-10-11 03:30:50 -0600

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-10-11 03:30:31 -0600

AlexandrosNic gravatar image

Instead of manually terminating the node, I found out that actually the state machine did not terminate automatically (as it should) because there was a blocked spin.

The solution I found was to add the "Grip" state in a MultiThreadedExecutor, using a ReentrantCallback, and then spin_once:

In the state:

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

the subscriber:

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

Then I destroyed the node and rclpy.shutdown()

This achieved the desired result: the state machine spins once, it goes through the callback, and then terminates.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2022-10-07 07:13:35 -0600

Seen: 54 times

Last updated: Oct 11 '22