1

在过去的几周里，我遇到了一个我仍然找不到解决方案的问题：

我执行一个从 pyscxml 启动 StateMachine 对象的 ROS 节点。该节点第一次收到来自另一个节点的消息时，会向 StateMachine 发送一个事件，并且一切正常。但是 SM 似乎仍然处于阻塞状态，并且下次当本地节点从另一个节点接收到新消息时，就没有机会将 SM 从之前的状态“移动”。好像SM对象总是一样的，但是ROS节点的thread id每次调用都会改变！！！？？？？？？我尝试了不同的选项来实例化 SM（@staticmethod、自己的线程等），但我找不到解决方案。

如果我使用创建匿名 Ros 节点的客户端并（例如每 3 秒）向运行 SM 的节点发送一条消息，它就可以工作！！SM 根据有任何问题的事件从一种状态更改为另一种状态。

当两个节点之间的连接关闭 x 秒并且我们想要向 SM 发送新事件时，问题就出现了。

这是我的 Ros 节点和 SM 模式定义。无论如何，scxml 模式似乎没有任何问题。

非常感谢您的帮助

#!/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

这是 SCXML 文档

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

<state id="INITIAL">
    <onentry>
        <log label="-----[SCXML-Main-Menu]" expr="'in INITIAL'" />
    </onentry>
    <transition event="startMenu" target="WAIT" />
</state>

<state id="WAIT">
    <onentry>
        <log label="-----[SCXML-Main-Menu]" expr="'in WAIT'" />
        <script>w
            declareStateMachineStatus("WAIT")
        </script>

    </onentry>
    <transition event="toMenu" target="MAIN_MENU" />
    <transition event="toFinal" target="FINAL" />
</state>


<state id="MAIN_MENU">
    <onentry>
        <log label="-----[SCXML-Main-Menu]" expr="'in MAIN_MENU'" />
        <script>
            declareStateMachineStatus("MAIN_MENU")
            triggerStartMainMenuEvent()
        </script>
    </onentry>
    <transition event="toWait" target="WAIT" />
    <transition event="toFinal" target="FINAL" />
</state>


<final id="FINAL">
    <onentry>
        <log label="-----[SCXML-Main-Menu]" expr="'in FINAL'" />
        <script>
            declareStateMachineStatus("FINAL")
        </script>
    </onentry>
</final>
4

0 回答 0