flexbe_input.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import roslib; roslib.load_manifest('flexbe_input')
4 import rospy
5 import pickle
6 import actionlib
7 import threading
8 
9 from flexbe_msgs.msg import BehaviorInputAction, BehaviorInputFeedback, BehaviorInputResult, BehaviorInputGoal
10 from .complex_action_server import ComplexActionServer
11 '''
12 Created on 02/13/2015
13 
14 @author: Philipp Schillinger, Brian Wright
15 '''
16 
17 class BehaviorInput(object):
18 
19  def __init__(self):
20  '''
21  Constructor
22  '''
23  #onboard connection
24  self._as = ComplexActionServer('flexbe/behavior_input', BehaviorInputAction, execute_cb=self.execute_cb, auto_start = False)
25  self._as.start()
26 
27  rospy.loginfo("Ready for data requests...")
28 
29  def execute_cb(self, goal , goal_handle):
30  rospy.loginfo("--> Got a request!")
31  rospy.loginfo('"%s"' % goal.msg)
32 
33  relay_ocs_client_ = actionlib.SimpleActionClient('flexbe/operator_input', BehaviorInputAction)
34 
35  # wait for data msg
36  print("waiting")
37  relay_ocs_client_.wait_for_server()
38  print("done")
39 
40  # Fill in the goal here
41  relay_ocs_client_.send_goal(goal)
42  print("waiting for result")
43  relay_ocs_client_.wait_for_result()
44  print("got result")
45 
46  result = BehaviorInputResult()
47  result = relay_ocs_client_.get_result()
48  #result.data now serialized
49  data_str = result.data
50  print(data_str)
51 
52  if(result.result_code == BehaviorInputResult.RESULT_OK):
53  self._as.set_succeeded(BehaviorInputResult(result_code=BehaviorInputResult.RESULT_OK, data=data_str), "ok",goal_handle)
54 
55  elif(result.result_code == BehaviorInputResult.RESULT_FAILED):
56  # remove
57  self._as.set_succeeded(BehaviorInputResult(result_code=BehaviorInputResult.RESULT_FAILED, data=data_str),"failed",goal_handle)
58  rospy.loginfo("<-- Replied with FAILED")
59 
60  elif(result.result_code == BehaviorInputResult.RESULT_ABORTED ):
61  self._as.set_succeeded(BehaviorInputResult(result_code=BehaviorInputResult.RESULT_ABORTED, data=data_str),"Aborted",goal_handle)
62  rospy.loginfo("<-- Replied with ABORT")
63 
64 
65 
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 
76 
77 
78 
79 
80 
def execute_cb(self, goal, goal_handle)
Definition: flexbe_input.py:29


flexbe_input
Author(s): Philipp Schillinger
autogenerated on Sun Dec 13 2020 04:01:40