def __init__(self):
states=['solid', 'liquid', 'gas', 'plasma']
Machine.__init__(self, states=states, initial='solid')
self.add_transition('melt', 'solid', 'liquid')
self.add_transition('evaporate', 'liquid', 'gas')
self.add_transition('sublimate', 'solid', 'gas')
self.add_transition('ionize', 'gas', 'plasma')
def __init__(self, config):
self.config = config
self.id = str(uuid.uuid4())
self.scene_id = config.get('scene_id', 'base')
self.name = config['name']
self._load_states(config)
self.transitions = config['transitions']
self.policy = Policy(config)
Machine.__init__(self,
model=self.policy,
states=self.states,
transitions=self.transitions,
initial=self.init_state,
after_state_change=['instruct'])
self._load_state_inputs()
def __init__(self):
rospy.init_node('pick_place_logic')
# initialize state machine
# state_machine_config = rospy.get_param('/ipr/state_machine_config')
# GraphMachine.__init__(self, title='Pick and Place State Manager', **state_machine_config)
file_stream = open(state_machine_file_path, 'r')
state_machine_config = yaml.load(file_stream)
GraphMachine.__init__(self, title='Pick and Place State Manager',
states=state_machine_config['states'],
transitions=state_machine_config['transitions'],
initial=state_machine_config['initial'])
# Moveit
moveit_commander.roscpp_initialize(sys.argv)
self._group = moveit_commander.MoveGroupCommander(self.GROUP_NAME)
self._robot = moveit_commander.RobotCommander()
self._motion_result_code = None
self.home_pos = HOME
# self.home_pos = self._group.get_current_pose().pose
self.prepick_pos = PREPICK
self.pick_pos = PICK
self.preplace_pos = PREPLACE
self.place_pos = PLACE
self.waypoints = []
self.job_finish = False
# Subscriber
rospy.Subscriber('/robot_status', RobotStatus, self._get_robot_state)
rospy.Subscriber('/execute_trajectory/result', moveit_msgs.msg.ExecuteTrajectoryActionResult, self._motion_result_callback)
rospy.Subscriber('/start_pick_place', Empty, self.start_pick_and_place)
# Publisher
self._state_machine_pub = rospy.Publisher('/state_machine/state', String, queue_size=10)
self._job_status_pub = rospy.Publisher('/job_status', String, queue_size=10)
# Event thread
self._robot_status_event = threading.Event()
self._motion_result_event = threading.Event()
self._robot_status_event.wait()
rospy.loginfo('Robot is initialized')
self.trig_initialized()
def __init__(self, bot):
self.bot = bot
self.anger_bot = AngerMeasure()
self.echo_count = 0
self.reply = None
"""
Each task includes states that trigger some user-defined processes and a state with suffix '_q'
which calls trigger 'Success' to move back to state 'base' once entered.
每個不同的任務都包含某些具有特定目的的state以及一個以'_q'為結尾的state，
一旦進入以'_q'為結尾的state會自動呼叫'Success'以回到初始狀態'base'
"""
self.states = [{
'name': 'dummy'
}, {
'name': 'annoyed',
'on_enter': 'Complaining'
}, {
'name': 'help',
'on_enter': 'showCommand'
}, {
'name': 'base',
'on_enter': 'settingUp'
}, {
'name': 'echo',
'on_enter': 'Echoing'
}, {
'name': 'echo_q',
'on_enter': 'Success_echo'
}, {
'name': 'gallary',
'on_enter': 'gallaryProcess'
}, {
'name': 'gallary_ask',
'on_enter': 'askMorePic'
}, {
'name': 'gallary_q',
'on_enter': 'Success_gallary'
}, {
'name': 'map',
'on_enter': 'askGPS'
}, {
'name': 'map_list',
'on_enter': 'ListFood'
}, {
'name': 'map_ask',
'on_enter': 'askMoreFood'
}, {
'name': 'map_load',
'on_enter': 'updateFood'
}, {
'name': 'map_q',
'on_enter': 'Success_map'
}, {
'name': 'file',
'on_enter': 'askDownload'
}, {
'name': 'file_n_q',
'on_enter': 'Success_file_n'
}, {
'name': 'file_q',
'on_enter': 'Success_file'
}]
"""
似乎越早定義的conditions優先權越高，像是Reboot的優先權就是最大的，只要他回傳True，不論下面其他conditions說什麼都沒用
"""
self.transitions = [
{
'trigger': 'Advance',
'source': '*',
'dest': 'base',
'conditions': 'Reboot'
},
{
'trigger': 'Annoyed',
'source': '*',
'dest': 'annoyed'
},
{
'trigger': 'Success',
'source': '*',
'dest': 'base',
'before': 'updateReply'
},
{
'trigger': 'Advance',
'source': 'base',
'dest': 'echo',
'conditions': 'isTextSticker',
'unless': 'isTaskKw'
},
{
'trigger': 'Advance',
'source': 'echo',
'dest': '=',
'after': 'EchoCount',
'unless': ['quitEcho']
},
{
'trigger': 'Advance',
'source': 'echo',
'dest': 'echo_q',
'conditions': 'quitEcho'
},
{
'trigger': 'Advance',
'source': 'base',
'dest': 'help',
'conditions': 'askHelp'
},
{
'trigger': 'Advance',
'source': 'base',
'dest': 'gallary',
'conditions': 'askPic'
},
{
'trigger': 'Jump',
'source': 'gallary',
'dest': 'gallary_ask'
},
{
'trigger': 'Advance',
'source': 'gallary_ask',
'dest': 'gallary_q',
'conditions': 'receiveNo'
},
{
'trigger': 'Advance',
'source': 'gallary_ask',
'dest': 'gallary',
'conditions': 'receiveOk'
},
{
'trigger': 'Advance',
'source': 'gallary_ask',
'dest': '=',
'conditions': 'Digress'
},
{
'trigger': 'Advance',
'source': 'base',
'dest': 'map',
'conditions': 'Starving'
},
{
'trigger': 'Advance',
'source': 'map',
'dest': 'map_list',
'conditions': 'receiveGPS'
},
{
'trigger': 'Advance',
'source': 'map',
'dest': '=',
'unless': ['receiveGPS']
},
{
'trigger': 'Jump',
'source': 'map_list',
'dest': 'map_ask'
},
{
'trigger': 'Advance',
'source': 'map_ask',
'dest': 'map_load',
'conditions': ['receiveOk', 'existMore']
},
{
'trigger': 'Jump',
'source': 'map_load',
'dest': 'map_list'
},
{
'trigger': 'Advance',
'source': 'map_ask',
'dest': 'map_q',
'conditions': 'receiveNo'
},
{
'trigger': 'Advance',
'source': 'map_ask',
'dest': 'map_q',
'unless': ['existMore']
},
{
'trigger': 'Advance',
'source': 'map_ask',
'dest': '=',
'conditions': 'Digress'
},
{
'trigger': 'Advance',
'source': 'base',
'dest': 'file',
'unless': 'isTextSticker'
},
{
'trigger': 'Advance',
'source': 'file',
'dest': 'file_n_q',
'conditions': 'receiveNo'
},
{
'trigger': 'Advance',
'source': 'file',
'dest': '=',
'conditions': 'Digress'
}, #digress=離題
{
'trigger': 'Advance',
'source': 'file',
'dest': 'file_q',
'conditions': 'receiveOk'
}
]
Machine.__init__(self,
states=self.states,
transitions=self.transitions,
initial='dummy',
send_event=True,
show_conditions=True)
