0
$\begingroup$

Rosanswers logo

Hello,

I am using a smach concurrent state machine and I want it to transition, even if one of the child states has died. It seems like it sits there waiting for the pre-emption to return, when it never will, since the process has died. This leads to undesirable behavior where the state machine will not transition to other states when command messages are sent.

My concurrent state is composed of the following states:

  • A simple action state that executes a long running behavior, implemented as a ROS nodelet.
  • A monitor state that subscribes to a ROS topic, and transitions when that topic is received.

This all works fine if the nodelet is running as expected. I can publish a topic which the monitor state listens for, the monitor state becomes invalid, and terminates, causing the outcome of the concurrent state to transition to a different state.

The problem comes when the simple action state has died (due to software bugs causing a crash.) Of course, I am fixing these bugs to prevent future crashes of the nodelet process. I am also now setting the problem nodelet to respawn in the launch file. However, for robustness, I would like for the state machine to still transition, even if this process has died.

Is this behavior by design or is there a way to force the concurrence behavior to transition, even if one of the children does not respond to the pre-emption request? In my outcome map, I am using the following, which I thought would cause it to terminate from the monitor state alone, but I think it is waiting for a response from the crashed nodelet.

child_termination_cb=(lambda so: True)

Thanks in advance!

Edit: I am adding my subclassed code for SimpleActionState: Edit2: The code is working now and properly aborts the state:

import roslib
import rospy
import smach
import smach_ros
from smach import State, StateMachine, CBState
from smach_ros import SimpleActionState, IntrospectionServer
from std_msgs.msg import Empty

import threading
import traceback
import copy

from actionlib.simple_action_client import SimpleActionClient, GoalStatus

import smach
from smach.state import *

__all__ = ['WatchdogActionState']

class WatchdogActionState(SimpleActionState):
    """Watchdog Simple action client state.  
       Subclasses the Smach SimpleActionState to provide watchdog capability on the actionserver process execution.
    """
    
"""Constructor for SimpleActionState action client wrapper."""
# Overridden constructor:
def __init__(self,
        # Action info
        action_name,
        action_spec, 
        # Feedback (heartbeat) timeout = how long to wait for a process that has died
        # before aborting this state.
        feedback_wait_timeout = rospy.Duration(3.0),
        # Default goal 
        goal = None,
        goal_key = None,
        goal_slots = [],
        goal_cb = None,
        goal_cb_args = [],
        goal_cb_kwargs = {},
        # Result modes
        result_key = None,
        result_slots = [],
        result_cb = None,
        result_cb_args = [],
        result_cb_kwargs = {},
        # Keys
        input_keys = [],
        output_keys = [],
        outcomes = ['succeeded','aborted','preempted'],
        # Timeouts
        exec_timeout = None,
        preempt_timeout = rospy.Duration(1.0),
        server_wait_timeout = rospy.Duration(1.0)
        ):

# Initialize base class
rospy.loginfo("init watchdog_action_state for action_name [%s], action_spec [%s]", action_name, action_spec)
SimpleActionState.__init__(self,
            action_name,
            action_spec, 
            goal,
            goal_key,
            goal_slots,
            goal_cb,
            goal_cb_args,
            goal_cb_kwargs,
            result_key,
            result_slots,
            result_cb,
            result_cb_args,
            result_cb_kwargs,
            input_keys,
            output_keys,
            outcomes,
            exec_timeout,
            preempt_timeout,
            server_wait_timeout)
    # The feedback (hearbeat) timeout
    self._feedback_wait_timeout = feedback_wait_timeout
rospy.loginfo("server_wait_timeout [%f]", self._feedback_wait_timeout.to_sec())
    self._feedback_time = rospy.Time.now()
    self._heartbeat_timer_thread = None

# New Method/thread: _heartbeat_timer()
# Aborts the state if we don't receive feedback after a given amount of time.
# This could happen if our SimpleActionState nodelet becomes non-responsive.
def _heartbeat_timer(self): 
"""Internal method for cancelling a timed out feedback after a timeout.""" 
    rospy.logwarn("1")        
while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown():  
    try: 
        rospy.sleep(0.1) 
    except: 
    if not rospy.is_shutdown(): 
        rospy.logerr("Failed to sleep while running '%s'" % self._action_name) 
    if rospy.Time.now() - self._feedback_time > self._feedback_wait_timeout: 
        # Abort the goal and state 
            rospy.logwarn("Attempting to abort state.")
            self._goal_result = 1 # Aborted 
            self._goal_done_cb(1, 'ABORTED')
            self._goal_status == GoalStatus.ABORTED
            self.outcome = 'aborted'
            return

# Overridden method: execute()
# We override this so that we can catch the case where the Simple Action server (nodelet) is not running
# or has crashed, so that we don't block waiting for a response.
def execute(self, ud):
    """Called when executing a state.
    This calls the goal_cb if it is defined, and then dispatches the
    goal with a non-blocking call to the action client.
    """
    # Assume that the server should be running and if we get to this point, it is not responsive, so abort.
    if self._status is SimpleActionState.WAITING_FOR_SERVER:
        rospy.logfatal("ExecuteCB: Action server for "+self._action_name+" is not running.")
        return 'aborted'
    else:
        #  Make sure that the state is active
        self._status = SimpleActionState.ACTIVE
        # Start the heartbeat timeout watch thread 
        self._heartbeat_timer_thread = threading.Thread(name=self._action_name+'/heartbeat_watchdog', target=self._heartbeat_timer) 
        self._heartbeat_timer_thread.start() 

        # Let the parent SimpleActionState class processing proceed as normal.
        return SimpleActionState.execute(self, ud)
 
### Action client callbacks
# Overridden method: _goal_active_cb() to allow for logging
# We update our run timers here to allow us to abort if there is a state crash
def _goal_active_cb(self):
    """Goal Active Callback
    This callback starts the timer that watches for the timeout specified for this action.
    """
    # Keep track of when we started the process, for the heartbeat
    self._feedback_time = rospy.Time.now()
    rospy.loginfo("Action "+self._action_name+" has gone active, waiting for feedback/heartbeats.")

# Overridden method: _goal_feedback_cb() to allow for logging
# We update our run timers here to allow us to abort if there is a state crash
def _goal_feedback_cb(self, feedback):
    """Goal Feedback Callback"""
    # Keep track of when we last heard from the process, for the heartbeat
    self._feedback_time = rospy.Time.now()
    rospy.logdebug("Action "+self._action_name+" sent feedback.")

Originally posted by ceverett on ROS Answers with karma: 332 on 2015-06-11

Post score: 1


Original comments

Comment by ceverett on 2015-06-11:
I found a related issue, but it doesn't say how to address the problem with smach: simpleactionstate-behaviour-if-action-server-crashes

Comment by ceverett on 2015-06-22:
I have it working now. I pasted the correct code now, which subclasses SimpleActionState and will abort the state in a separate heartbeat thread if it hasn't received feedback in 3 seconds. If you have any feedback that would be great, but I am marking this as answered. Thanks again.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You would need to create a NotSoSimpleActionState which accepts a timeout that can be used to monitor the actionlib status topic for the action. Have the state abort if this heartbeat timeout is violated.

The problem is that when the concurrence tries to preempt the SimpleActionState, it's trying to preempt the action, which will never preempt, because it has died.


Originally posted by jbohren with karma: 5809 on 2015-06-11

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by ceverett on 2015-06-19:
Thank you very much. I added the code for my first pass at this suggestion to the question above. However, I am not sure how I can abort? Please take a look at the method/thread _heartbeat_timer() where I try to abort. What should I call to force an abort? Thanks!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.