Integrating SCXML in ROS using PYSCXML [closed]

edit

Hi everyone,

In the last weeks a faced a problem that still I could not find a solution:

I execute a ROS node that starts a StateMachine object from pyscxml. The first time that this node receives a message from another node, an event is send to the StateMachine and everything works fine. But it seems the SM remains in a blocked status, and the next time when the local node where the SM was started, receives a new message from another node, there is no more a chance to 'move' the SM from the previous state. It seems that the SM object is the same always, but the thread id of the ROS node changes in every call !!!????? I tried different options to instantiate the SM (@staticmethod, own thread, etc), but I could not find a solution.

If I use a client that creates a anonymous Ros node and (e.g. every 3 seconds) sends a message to the Node where the SM runs, it works!! the SM change from a state to another one depending on the event.

The problem comes when the connection between the two nodes is closed for x seconds and we want to send new events to the SM.

At the bottom you can see my Ros Node and the SM schema definition. Anyway it does not seem that the scxml schema has any problem.

Thank you very much for your help

#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
Created on 07/12/2012

@author: frk
'''
import roslib; roslib.load_manifest('beginner_tutorials')
from beginner_tutorials.de.frank.dm.scxml.pyscxml import StateMachine
from beginner_tutorials.de.frank.utils.Logger import LogUtils
from beginner_tutorials.de.frank.utils.constants import RosChannel
from std_msgs.msg._String import String
import rospy
import time

LOG = LogUtils("WorkFlowManager", "stateMachine.log")
sm = None
event = None
receivedEvent = None

## NODE initialization
rospy.init_node(RosChannel.NODE_WORKFLOW)

## Publishers ##
pubMainmenu = rospy.Publisher(RosChannel.TOPIC_START_MENU, String) 
pubMusicBox = rospy.Publisher(RosChannel.TOPIC_MUSICBOX, String)

STATE_WAIT = "WAIT"
STATE_RUN = "RUN"
STATE_MUSIC = "MUSIC"
STATE_NEWS = "NEWS"
STATE_FINAL = "FINAL"
STATE_MAIN_MENU = "MAIN_MENU"

def init_machine():
    global sm
    sm = StateMachine("workflowMain.xml")
    sm.start_threaded()
    return sm    

def getStateMachine():
    global sm
    if None != sm:
        return sm
    else:
        return init_machine()

def parseRosMsg(message):
    event = str(message.data)
    sendSafeEventToMachine(event)
    LOG.debug("[" + getState() + "]" + " AFTER sending '" + event + "'")

def declareStateMachineStatus(value):
    rospy.set_param(RosChannel.P_STATE_MACHINE_STATUS, value)

def getState():
    return str(rospy.get_param(RosChannel.P_STATE_MACHINE_STATUS))

def sendSafeEventToMachine(event):
    global receivedEvent
    receivedEvent = False
    index = 0
    LOG.debug("[" + getState() + "] Sending to Main STATE MACHINE event: '" + event + "'")
    while index < 2: # this is a workaround, it avoids the issue sending sometimes evens that are obviated by the sm
        getStateMachine().send(event, 0)
        time.sleep(0.5)
        index = index + 0.5

def triggerStartMainMenuEvent():
    LOG.debug("[" + getState() + "] Publishing 'startMenu'")
    pubMainmenu.publish(str("startMenu"))



if __name__ == '__main__':
    try:

        LOG.debug("Started nodeWorkflow with STATE 'WAIT'")
        rospy.set_param(RosChannel.P_STATE_MACHINE_STATUS, STATE_WAIT)

        ## Subscribers ##
        rospy.Subscriber(RosChannel.TOPIC_WORKFLOW, String, parseRosMsg)
        rospy.Subscriber(RosChannel.TOPIC_JULIUS, String, parseRosMsg)

        rospy.spin()

    except rospy.ROSInterruptException:
        pass  

And this is the StateMachine Schema

<script>
    from beginner_tutorials.de.frank.dm.mainWorkflowManager import *
</script ...