3333
3434import difflib
3535import os
36+ import time
3637import zlib
3738
3839from flexbe_core import BehaviorLibrary
4142from flexbe_msgs .action import BehaviorExecution
4243from flexbe_msgs .msg import BEStatus , BehaviorModification , BehaviorSelection
4344
45+ import rclpy
4446from rclpy .action import ActionServer
4547from rclpy .action .server import ServerGoalHandle
48+ from rclpy .callback_groups import MutuallyExclusiveCallbackGroup
4649
4750from rosidl_runtime_py import get_interface_path
4851
4952from std_msgs .msg import Empty , Int32
5053
5154import yaml
5255
53- import rclpy
54- import time
55- from rclpy .callback_groups import MutuallyExclusiveCallbackGroup
5656
5757class BehaviorActionServer :
5858 """Behavior action server."""
@@ -72,8 +72,10 @@ def __init__(self, node):
7272 self ._pub = self ._node .create_publisher (BehaviorSelection , Topics ._START_BEHAVIOR_TOPIC , 100 )
7373 self ._preempt_pub = self ._node .create_publisher (Empty , Topics ._CMD_PREEMPT_TOPIC , 100 )
7474
75- self ._status_pub = self ._node .create_subscription (BEStatus , Topics ._ONBOARD_STATUS_TOPIC , self ._status_cb , 100 , callback_group = self .topic_group )
76- self ._state_pub = self ._node .create_subscription (Int32 , Topics ._BEHAVIOR_UPDATE_TOPIC , self ._state_cb , 100 , callback_group = self .topic_group )
75+ self ._status_pub = self ._node .create_subscription (BEStatus , Topics ._ONBOARD_STATUS_TOPIC , self ._status_cb ,
76+ 100 , callback_group = self .topic_group )
77+ self ._state_pub = self ._node .create_subscription (Int32 , Topics ._BEHAVIOR_UPDATE_TOPIC , self ._state_cb ,
78+ 100 , callback_group = self .topic_group )
7779
7880 self ._as = ActionServer (self ._node , BehaviorExecution ,
7981 Topics ._EXECUTE_BEHAVIOR_ACTION ,
@@ -87,8 +89,7 @@ def __init__(self, node):
8789 self ._node .get_logger ().info ('%d behaviors available, ready for start request.' % self ._behavior_lib .count_behaviors ())
8890 self .running = False
8991
90-
91- def _goal_cb (self , goal_handle : ServerGoalHandle ):
92+ def _goal_cb (self , goal_handle : ServerGoalHandle ):
9293 self ._current_goal = goal_handle
9394 goal = goal_handle .request
9495
@@ -179,6 +180,7 @@ def _preempt_cb(self, goal_handle):
179180 self ._node .get_logger ().info ('Behavior execution preempt requested!' )
180181
181182 def clean_me (self ):
183+ """Clean up flags."""
182184 self .running = False
183185 self ._current_state = None
184186 self ._behavior_started = False
@@ -189,18 +191,18 @@ def _execute_cb(self, goal_handle):
189191
190192 while rclpy .ok () and self .running :
191193 time .sleep (0.01 )
192- print (" End execution" )
194+ print (' End execution' )
193195 self .clean_me ()
194196 result = BehaviorExecution .Result ()
195197 result .outcome = self .outcome
196- self .outcome = ""
198+ self .outcome = ''
197199 return result
198200
199201 def _status_cb (self , msg ):
200202 if msg .code == BEStatus .ERROR :
201203 self ._node .get_logger ().error ('Failed to run behavior! Check onboard terminal for further infos.' )
202204 self ._current_goal .abort ()
203- self .outcome = " error"
205+ self .outcome = ' error'
204206 self .clean_me ()
205207 return
206208
@@ -225,16 +227,16 @@ def _status_cb(self, msg):
225227 self ._node .get_logger ().info ("Finished behavior execution with result '%s'!" % result )
226228 if result == 'preempted' :
227229 self ._current_goal .succeed () # .canceled()
228- self .outcome = " preempted"
230+ self .outcome = ' preempted'
229231 self .clean_me ()
230232 else :
231233 self ._current_goal .succeed ()
232- self .outcome = " success"
234+ self .outcome = ' success'
233235 self .clean_me ()
234236 elif msg .code == BEStatus .FAILED :
235237 self ._node .get_logger ().error ("Behavior execution failed in state '%s'!" % str (self ._current_state ))
236238 self ._current_goal .abort ()
237- self .outcome = " failed"
239+ self .outcome = ' failed'
238240 self .clean_me ()
239241
240242 def _state_cb (self , msg ):
0 commit comments