input_state.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 import actionlib
5 import pickle
6 
7 from flexbe_core import EventState, Logger
8 from flexbe_msgs.msg import BehaviorInputAction, BehaviorInputGoal, BehaviorInputResult
9 from flexbe_core.proxy import ProxyActionClient
10 
11 '''
12 Created on 02/13/2015
13 
14 @author: Philipp Schillinger
15 '''
16 
17 class InputState(EventState):
18  '''
19  Implements a state where the state machine needs an input from the operator.
20  Requests of different types, such as requesting a waypoint, a template, or a pose, can be specified.
21 
22  -- request uint8 One of the class constants to specify the type of request.
23  -- message string Message displayed to the operator to let him know what to do.
24 
25  #> data object The data provided by the operator. The exact type depends on the request.
26 
27  <= received Returned as soon as valid data is available.
28  <= aborted The operator declined to provide the requested data.
29  <= no_connection No request could be sent to the operator.
30  <= data_error Data has been received, but could not be deserialized successfully.
31 
32  '''
33 
34  POINT_LOCATION = BehaviorInputGoal.POINT_LOCATION
35  SELECTED_OBJECT_ID = BehaviorInputGoal.SELECTED_OBJECT_ID
36  WAYPOINT_GOAL_POSE = BehaviorInputGoal.WAYPOINT_GOAL_POSE
37  GHOST_JOINT_STATES = BehaviorInputGoal.GHOST_JOINT_STATES
38  FOOTSTEP_PLAN_HEADER = BehaviorInputGoal.FOOTSTEP_PLAN_HEADER
39 
40 
41  def __init__(self, request, message):
42  '''
43  Constructor
44  '''
45  super(InputState, self).__init__(outcomes=['received', 'aborted', 'no_connection', 'data_error'],
46  output_keys=['data'])
47 
48  self._action_topic = 'flexbe/behavior_input'
49 
50  self._client = ProxyActionClient({self._action_topic: BehaviorInputAction})
51 
52  self._request = request
53  self._message = message
54  self._connected = True
55  self._received = False
56 
57 
58  def execute(self, userdata):
59  '''
60  Execute this state
61  '''
62  if not self._connected:
63  return 'no_connection'
64  if self._received:
65  return 'received'
66 
67  if self._client.has_result(self._action_topic):
68  result = self._client.get_result(self._action_topic)
69  if result.result_code != BehaviorInputResult.RESULT_OK:
70  userdata.data = None
71  return 'aborted'
72  else:
73  try:
74  response_data = pickle.loads(result.data)
75  #print 'Input request response:', response_data
76  userdata.data = response_data
77  # If this was a template ID request, log that template ID:
78  if self._request == BehaviorInputGoal.SELECTED_OBJECT_ID:
79  Logger.loginfo('Received template ID: %d' % int(response_data))
80 
81  except Exception as e:
82  Logger.logwarn('Was unable to load provided data:\n%s' % str(e))
83  userdata.data = None
84  return 'data_error'
85 
86  self._received = True
87  return 'received'
88 
89 
90  def on_enter(self, userdata):
91 
92  self._received = False
93 
94  if not self._connected: return
95 
96  action_goal = BehaviorInputGoal()
97  action_goal.request_type = self._request
98  action_goal.msg = self._message
99 
100  try:
101  self._client.send_goal(self._action_topic, action_goal)
102  except Exception as e:
103  Logger.logwarn('Was unable to send data request:\n%s' % str(e))
104  self._connected = False
105 
106 
107 
108 
109 
110 
111 
112 
113 
114 
115 
116 
117 
118 
119 
def __init__(self, request, message)
Definition: input_state.py:41


flexbe_states
Author(s): Philipp Schillinger
autogenerated on Wed Jun 5 2019 21:52:08