class complete_task_StartHack(EventState): ''' Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. -- target_time float Time which needs to have passed since the behavior started. <= continue Given time has passed. <= failed Example for a failure outcome. ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(complete_task_StartHack, self).__init__(outcomes = ['error','done'], input_keys = ['task_details_task_id']) self._topic_set_task_state = 'taskallocation/set_task_state' self._pub = ProxyPublisher({self._topic_set_task_state: SetTaskState}) self._task_state_completed = TaskState() self._task_state_completed.state = TaskState.COMPLETED def execute(self, userdata): request = SetTaskStateRequest() request.task_id = userdata.task_details_task_id request.new_state = self._task_state_completed try: self._pub.publish(self._topic_set_task_state, request) return 'done' except Exception, e: Logger.logwarn('Could not set task state:' + str(e)) return 'error'
class PublishPoseState(EventState): """ Publishes a pose from userdata so that it can be displayed in rviz. -- topic string Topic to which the pose will be published. ># pose PoseStamped Pose to be published. <= done Pose has been published. """ def __init__(self, topic): """Constructor""" super(PublishPoseState, self).__init__(outcomes=['done'], input_keys=['pose']) self._topic = topic self._pub = ProxyPublisher({self._topic: PoseStamped}) def execute(self, userdata): return 'done' def on_enter(self, userdata): self._pub.publish(self._topic, userdata.pose)
class ShowPictureWebinterfaceState(EventState): ''' Displays the picture in a web interface ># Image Image The received Image <= done Displaying the Picture ''' def __init__(self): super(ShowPictureWebinterfaceState, self).__init__(outcomes = ['tweet', 'forget'], input_keys = ['image_name']) self._pub_topic = '/webinterface/display_picture' self._pub = ProxyPublisher({self._pub_topic: String}) self._sub_topic = '/webinterface/dialog_feedback' self._sub = ProxySubscriberCached({self._sub_topic: String}) def execute(self, userdata): if self._sub.has_msg(self._sub_topic): msg = self._sub.get_last_msg(self._sub_topic) if msg.data == 'yes': print 'show_picture_webinterface_state, returning tweet' return 'tweet' else: print 'show_picture_webinterface_state, returning forget' return 'forget' def on_enter(self,userdata): self._sub.remove_last_msg(self._sub_topic) self._pub.publish(self._pub_topic, String(userdata.image_name))
class PreemptableState(LoopbackState): """ A state that can be preempted. If preempted, the state will not be executed anymore and return the outcome preempted. """ _preempted_name = "preempted" preempt = False switching = False def __init__(self, *args, **kwargs): # add preempted outcome if len(args) > 0 and type(args[0]) is list: # need this ugly check for list type, because first argument in CBState is the callback args[0].append(self._preempted_name) else: outcomes = kwargs.get("outcomes", []) outcomes.append(self._preempted_name) kwargs["outcomes"] = outcomes super(PreemptableState, self).__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._preemptable_execute self._feedback_topic = "/flexbe/command_feedback" self._preempt_topic = "/flexbe/command/preempt" self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _preemptable_execute(self, *args, **kwargs): preempting = False if self._is_controlled and self._sub.has_msg(self._preempt_topic): self._sub.remove_last_msg(self._preempt_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt")) preempting = True rospy.loginfo("--> Behavior will be preempted") if PreemptableState.preempt: PreemptableState.preempt = False preempting = True rospy.loginfo("Behavior will be preempted") if preempting: self.service_preempt() self._force_transition = True return self._preempted_name return self.__execute(*args, **kwargs) def _enable_ros_control(self): super(PreemptableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._preempt_topic, Empty) PreemptableState.preempt = False def _disable_ros_control(self): super(PreemptableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._preempt_topic)
class PauseExploration(EventState): ''' Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. -- target_time float Time which needs to have passed since the behavior started. <= continue Given time has passed. <= failed Example for a failure outcome. ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(PauseExploration, self).__init__(outcomes = ['continue', 'pause']) self._topic_cancel = 'move_base/cancel' self._pub = ProxyPublisher({self._topic_cancel: Empty}) def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. return 'pause' # One of the outcomes declared above. def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._pub.publish(self._topic_cancel, Empty()) def on_exit(self, userdata): # This method is called when an outcome is returned and another state gets active. # It can be used to stop possibly running processes started by on_enter. pass def on_start(self): # This method is called when the behavior is started. # If possible, it is generally better to initialize used resources in the constructor # because if anything failed, the behavior would not even be started. pass def on_stop(self): # This method is called whenever the behavior stops execution, also if it is cancelled. # Use this event to clean up things like claimed resources. pass
class RobotStateCommandState(EventState): ''' Publishes a robot state command message. -- command int Command to be sent. Use class variables (e.g. STAND). <= done Successfully published the state command message. <= failed Failed to publish the state command message. ''' START_HYDRAULIC_PRESSURE_OFF = 4 # "fake" start (without hydraulic pressure) START_HYDRAULIC_PRESSURE_ON = 6 # start normally (with hydraulic pressure) STOP = 8 # stop the pump FREEZE = 16 STAND = 32 STAND_PREP = 33 CALIBRATE = 64 # BIASES CALIBRATE_ARMS = 128 CALIBRATE_ARMS_FREEZE = 144 def __init__(self, command): '''Constructor''' super(RobotStateCommandState, self).__init__(outcomes = ['done', 'failed']) self._topic = '/flor/controller/robot_state_command' self._pub = ProxyPublisher({self._topic: FlorRobotStateCommand}) self._command = command self._failed = False def execute(self, userdata): if self._failed: return 'failed' else: return 'done' def on_enter(self, userdata): self._failed = False try: command_msg = FlorRobotStateCommand(state_command = self._command) self._pub.publish(self._topic, command_msg) except Exception as e: Logger.logwarn('Failed to publish the command message:\n%s' % str(e)) self._failed = True
class VideoLoggingState(EventState): """ A state that controls video logging. -- command boolean One of the available commands provided as class constants. -- no_video boolean Only create bag files. -- no_bags boolean Only record video. ># experiment_name string Unique name of the experiment. <= done Executed the specified command. """ START = True STOP = False def __init__(self, command, no_video=False, no_bags=True): """Constructor""" super(VideoLoggingState, self).__init__(outcomes=['done'], input_keys=['experiment_name', 'description']) self._topic = "/vigir_logging" self._pub = ProxyPublisher({self._topic: OCSLogging}) self._command = command self._no_video = no_video self._no_bags = no_bags def execute(self, userdata): """Execute this state""" # nothing to check return 'done' def on_enter(self, userdata): """Upon entering the state""" try: self._pub.publish(self._topic, OCSLogging(run=self._command, no_video=self._no_video, no_bags=self._no_bags, experiment_name=userdata.experiment_name, description=userdata.description)) except Exception as e: Logger.logwarn('Could not send video logging command:\n %s' % str(e)) def on_stop(self): """Stop video logging upon end of execution""" try: self._pub.publish(self._topic, OCSLogging(run=False)) except Exception as e: pass
class ManuallyTransitionableState(MonitoringState): """ A state for that a desired outcome can be declared. If any outcome is declared, this outcome is forced. """ def __init__(self, *args, **kwargs): super(ManuallyTransitionableState, self).__init__(*args, **kwargs) self._force_transition = False self.__execute = self.execute self.execute = self._manually_transitionable_execute self._feedback_topic = '/flexbe/command_feedback' self._transition_topic = '/flexbe/command/transition' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _manually_transitionable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_buffered(self._transition_topic): command_msg = self._sub.get_from_buffer(self._transition_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="transition", args=[command_msg.target, self.name])) if command_msg.target != self.name: rospy.logwarn("--> Requested outcome for state " + command_msg.target + " but active state is " + self.name) else: self._force_transition = True outcome = self._outcome_list[command_msg.outcome] rospy.loginfo("--> Manually triggered outcome " + outcome + " of state " + self.name) return outcome # return the normal outcome self._force_transition = False return self.__execute(*args, **kwargs) def _enable_ros_control(self): super(ManuallyTransitionableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._transition_topic, OutcomeRequest) self._sub.enable_buffer(self._transition_topic) def _disable_ros_control(self): super(ManuallyTransitionableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._transition_topic)
class MirrorState(EventState): ''' This state will display its possible outcomes as buttons in the GUI and is designed in a way to be created dynamically. ''' def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): ''' Constructor ''' super(MirrorState, self).__init__(outcomes=given_outcomes) self._rate = rospy.Rate(100) self._given_outcomes = given_outcomes self._outcome_autonomy = outcome_autonomy self._target_name = target_name self._target_path = target_path self._outcome_topic = 'flexbe/mirror/outcome' self._pub = ProxyPublisher() #{'flexbe/behavior_update': String} self._sub = ProxySubscriberCached({self._outcome_topic: UInt8}) def execute(self, userdata): ''' Execute this state ''' if JumpableStateMachine.refresh: JumpableStateMachine.refresh = False self.on_enter(userdata) if self._sub.has_buffered(self._outcome_topic): msg = self._sub.get_from_buffer(self._outcome_topic) if msg.data < len(self._given_outcomes): rospy.loginfo("State update: %s > %s", self._target_name, self._given_outcomes[msg.data]) return self._given_outcomes[msg.data] try: self._rate.sleep() except ROSInterruptException: print 'Interrupted mirror sleep.' def on_enter(self, userdata): #rospy.loginfo("Mirror entering %s", self._target_path) self._pub.publish('flexbe/behavior_update', String("/" + "/".join(self._target_path.split("/")[1:])))
class confirm_victim(EventState): """ Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. -- target_time float Time which needs to have passed since the behavior started. <= continue Given time has passed. <= failed Example for a failure outcome. """ def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(confirm_victim, self).__init__(outcomes=["succeeded"], input_keys=["task_details_task_id"]) self._topicVictimReached = "victimReached" self._pub = ProxyPublisher({self._topicVictimReached: VictimAnswer}) def execute(self, userdata): answer = VictimAnswer() answer.task_id = userdata.task_details_task_id answer.answer = VictimAnswer.CONFIRM self._pub.publish(self._topicVictimReached, answer) Logger.loginfo("Victim Found, id = " + str(userdata.task_details_task_id)) return "succeeded" def on_enter(self, userdata): pass def on_exit(self, userdata): pass # Nothing to do in this example. def on_start(self): pass def on_stop(self): pass # Nothing to do in this example.
class SendToOperatorState(EventState): ''' Sends the provided data to the operator using a generic serialized topic. ># data object The data to be sent to the operator. You should be aware of required bandwidth consumption. <= done Data has been sent to onboard. This is no guarantee that it really has been received. <= no_connection Unable to send the data. ''' def __init__(self): '''Constructor''' super(SendToOperatorState, self).__init__(outcomes = ['done', 'no_connection'], input_keys = ['data']) self._data_topic = "/flexbe/data_to_ocs" self._pub = ProxyPublisher({self._data_topic: String}) self._failed = False def execute(self, userdata): if self._failed: return 'no_connection' return 'done' def on_enter(self, userdata): self._failed = False try: self._pub.publish(self._data_topic, String(pickle.dumps(userdata.data))) except Exception as e: Logger.logwarn('Failed to send data to OCS:\n%s' % str(e)) self._failed = True
class LookAtTargetState(EventState): ''' A state that can look at any link target, e.g. one of the hands or a template. ># frame string Frame to be tracked, pass None to look straight. <= done Command has been sent to head control. ''' def __init__(self): '''Constructor''' super(LookAtTargetState, self).__init__(outcomes=['done'], input_keys=['frame']) self._head_control_topic = '/thor_mang/head_control_mode' self._pub = ProxyPublisher({self._head_control_topic: HeadControlCommand}) def execute(self, userdata): '''Execute this state''' return 'done' def on_enter(self, userdata): '''Entering the state.''' command = HeadControlCommand() if userdata.frame is not None: command.motion_type = HeadControlCommand.TRACK_FRAME command.tracking_frame = userdata.frame else: command.motion_type = HeadControlCommand.LOOK_STRAIGHT self._pub.publish(self._head_control_topic, command)
class Send_Request(EventState): ''' Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. -- target_time float Time which needs to have passed since the behavior started. <= continue Given time has passed. <= failed Example for a failure outcome. ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Send_Request, self).__init__(outcomes = ['succeeded'], input_keys = ['pose','params','frameId']) #self.userdata.goalId = 'none' #self.userdata.frameId = frameId useMoveBase=True topic = 'move_base/observe' if useMoveBase else 'controller/goal' self._topic_move_base_goal = topic self._pub = ProxyPublisher({self._topic_move_base_goal: MoveBaseActionGoal}) def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. ##if rospy.Time.now() - self._start_time < self._target_time: return 'succeeded' # One of the outcomes declared above. def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. # The following code is just for illustrating how the behavior logger works. # Text logged by the behavior logger is sent to the operator and displayed in the GUI. ##time_to_wait = rospy.Time.now() - self._start_time - self._target_time ##if time_to_wait > 0: ##Logger.loginfo('Need to wait for %.1f seconds.' % time_to_wait) goal = MoveBaseActionGoal() goal.header.stamp = Time.now() goal.header.frame_id = userdata.frameId goal.goal.target_pose.pose.position = userdata.pose.position goal.goal.distance = userdata.params['distance'] # "straighten up" given orientation yaw = euler_from_quaternion([userdata.pose.orientation.x, userdata.pose.orientation.y, userdata.pose.orientation.z, self.pose.orientation.w])[2] goal.goal.target_pose.pose.orientation.x = 0 goal.goal.target_pose.pose.orientation.y = 0 goal.goal.target_pose.pose.orientation.z = math.sin(yaw/2) goal.goal.target_pose.pose.orientation.w = math.cos(yaw/2) goal.goal.target_pose.header.frame_id = userdata.frameId goal.goal_id.id = 'driveTo' + str(goal.header.stamp.secs) + '.' + str(goal.header.stamp.nsecs) goal.goal_id.stamp = goal.header.stamp userdata.goalId = goal.goal_id.id self._pub.publish(self._topic_move_base_goal, goal) return 'succeeded' def on_exit(self, userdata): # This method is called when an outcome is returned and another state gets active. # It can be used to stop possibly running processes started by on_enter. pass # Nothing to do in this example. def on_start(self): # This method is called when the behavior is started. # If possible, it is generally better to initialize used resources in the constructor # because if anything failed, the behavior would not even be started. # In this example, we use this event to set the correct start time. ##self._start_time = rospy.Time.now() pass def on_stop(self): # This method is called whenever the behavior stops execution, also if it is cancelled. # Use this event to clean up things like claimed resources. pass # Nothing to do in this example.
class VigirBeOnboard(object): ''' Implements an idle state where the robot waits for a behavior to be started. ''' def __init__(self): ''' Constructor ''' self.be = None Logger.initialize() smach.set_loggers( rospy.logdebug, # hide SMACH transition log spamming rospy.logwarn, rospy.logdebug, rospy.logerr) #ProxyPublisher._simulate_delay = True #ProxySubscriberCached._simulate_delay = True # prepare temp folder rp = rospkg.RosPack() self._tmp_folder = "/tmp/flexbe_onboard" if not os.path.exists(self._tmp_folder): os.makedirs(self._tmp_folder) sys.path.append(self._tmp_folder) # prepare manifest folder access self._behavior_lib = BehaviorLibrary() self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() self.status_topic = 'flexbe/status' self.feedback_topic = 'flexbe/command_feedback' self._pub.createPublisher(self.status_topic, BEStatus, _latch=True) self._pub.createPublisher(self.feedback_topic, CommandFeedback) # listen for new behavior to start self._running = False self._switching = False self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback) # heartbeat self._pub.createPublisher('flexbe/heartbeat', Empty) self._execute_heartbeat() rospy.sleep(0.5) # wait for publishers etc to really be set up self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') def _behavior_callback(self, msg): thread = threading.Thread(target=self._behavior_execution, args=[msg]) thread.daemon = True thread.start() def _behavior_execution(self, msg): if self._running: Logger.loginfo('--> Initiating behavior switch...') self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['received'])) else: Logger.loginfo('--> Starting new behavior...') be = self._prepare_behavior(msg) if be is None: Logger.logerr( 'Dropped behavior start request because preparation failed.') if self._running: self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) else: rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') return if self._running: if self._switching: Logger.logwarn('Already switching, dropped new start request.') return self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['start'])) if not self._is_switchable(be): Logger.logerr( 'Dropped behavior start request because switching is not possible.' ) self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable'])) return self._switching = True active_state = self.be.get_current_state() rospy.loginfo("Current state %s is kept active.", active_state.name) try: be.prepare_for_switch(active_state) self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['prepared'])) except Exception as e: Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e)) self._switching = False self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) return rospy.loginfo('Preempting current behavior version...') self.be.preempt_for_switch() rate = rospy.Rate(10) while self._running: rate.sleep() self._switching = False self._running = True self.be = be result = "" try: rospy.loginfo('Behavior ready, execution starts now.') rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum) self.be.confirm() args = [self.be.requested_state_path ] if self.be.requested_state_path is not None else [] self._pub.publish( self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args)) result = self.be.execute() if self._switching: self._pub.publish( self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING)) else: self._pub.publish( self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED, args=[str(result)])) except Exception as e: self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED)) Logger.logerr('Behavior execution failed!\n%s' % str(e)) import traceback Logger.loginfo(traceback.format_exc()) result = "failed" try: self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) self.be = None if not self._switching: rospy.loginfo('Behavior execution finished with result %s.', str(result)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') self._running = False def _prepare_behavior(self, msg): # get sourcecode from ros package try: rp = rospkg.RosPack() behavior = self._behavior_lib.get_behavior(msg.behavior_id) if behavior is None: raise ValueError(msg.behavior_id) be_filepath = os.path.join( rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py') if os.path.isfile(be_filepath): be_file = open(be_filepath, "r") rospy.logwarn( "Found a tmp version of the referred behavior! Assuming local test run." ) else: be_filepath = os.path.join( rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py') be_file = open(be_filepath, "r") be_content = be_file.read() be_file.close() except Exception as e: Logger.logerr('Failed to retrieve behavior from library:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # apply modifications if any try: file_content = "" last_index = 0 for mod in msg.modifications: file_content += be_content[last_index:mod. index_begin] + mod.new_content last_index = mod.index_end file_content += be_content[last_index:] if zlib.adler32(file_content) != msg.behavior_checksum: mismatch_msg = ( "Checksum mismatch of behavior versions! \n" "Attempted to load behavior: %s\n" "Make sure that all computers are on the same version a.\n" "Also try: rosrun flexbe_widget clear_cache" % str(be_filepath)) raise Exception(mismatch_msg) else: rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications)) except Exception as e: Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # create temp file for behavior class try: file_path = os.path.join(self._tmp_folder, 'tmp_%d.py' % msg.behavior_checksum) sc_file = open(file_path, "w") sc_file.write(file_content) sc_file.close() except Exception as e: Logger.logerr( 'Failed to create temporary file for behavior class:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # import temp class file and initialize behavior try: package = __import__("tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum]) clsmembers = inspect.getmembers( package, lambda member: inspect.isclass(member) and member. __module__ == package.__name__) beclass = clsmembers[0][1] be = beclass() rospy.loginfo('Behavior ' + be.name + ' created.') except Exception as e: Logger.logerr('Exception caught in behavior definition:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # import contained behaviors contain_list = {} try: contain_list = self._build_contains(be, "") except Exception as e: Logger.logerr('Failed to load contained behaviors:\n%s' % str(e)) return # initialize behavior parameters if len(msg.arg_keys) > 0: rospy.loginfo('The following parameters will be used:') try: for i in range(len(msg.arg_keys)): if msg.arg_keys[i] == '': # action call has empty string as default, not a valid param key continue key_splitted = msg.arg_keys[i].rsplit('/', 1) if len(key_splitted) == 1: behavior = '' key = key_splitted[0] rospy.logwarn( 'Parameter key %s has no path specification, assuming: /%s' % (key, key)) else: behavior = key_splitted[0] key = key_splitted[1] found = False if behavior == '' and hasattr(be, key): self._set_typed_attribute(be, key, msg.arg_values[i]) # propagate to contained behaviors for b in contain_list: if hasattr(contain_list[b], key): self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b) found = True for b in contain_list: if b == behavior and hasattr(contain_list[b], key): self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b) found = True if not found: rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not implemented') except Exception as e: Logger.logerr('Failed to initialize parameters:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # build state machine try: be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False) be.prepare_for_execution( self._convert_input_data(msg.input_keys, msg.input_values)) rospy.loginfo('State machine built.') except Exception as e: Logger.logerr('Behavior construction failed!\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return return be def _is_switchable(self, be): if self.be.name != be.name: Logger.logerr( 'Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s' % (self.be.name, be.name)) return False # locked inside # locked state exists in new behavior #if self.be.id == be.id: #Logger.logwarn('Behavior version ID is the same.') # Logger.logwarn('Skipping behavior switch, version ID is the same.') # return False # ok, can switch return True def _cleanup_behavior(self, behavior_checksum): del (sys.modules["tmp_%d" % behavior_checksum]) file_path = os.path.join(self._tmp_folder, 'tmp_%d.pyc' % behavior_checksum) try: os.remove(file_path) except OSError: pass try: os.remove(file_path + 'c') except OSError: pass def _set_typed_attribute(self, obj, name, value, behavior=''): attr = getattr(obj, name) if type(attr) is int: value = int(value) elif type(attr) is long: value = long(value) elif type(attr) is float: value = float(value) elif type(attr) is bool: value = (value != "0") elif type(attr) is dict: value = yaml.load(value) setattr(obj, name, value) suffix = ' (' + behavior + ')' if behavior != '' else '' rospy.loginfo(name + ' = ' + str(value) + suffix) def _convert_input_data(self, keys, values): result = dict() for k, v in zip(keys, values): try: result[k] = self._convert_dict(cast(v)) except ValueError: # unquoted strings will raise a ValueError, so leave it as string in this case result[k] = str(v) except SyntaxError as se: Logger.logwarn( 'Unable to parse input value for key "%s", assuming string:\n%s\%s', k, str(v), str(se)) result[k] = str(v) return result def _build_contains(self, obj, path): contain_list = dict( (path + "/" + key, value) for (key, value) in getattr(obj, 'contains', {}).items()) add_to_list = {} for b_id, b_inst in contain_list.items(): add_to_list.update(self._build_contains(b_inst, b_id)) contain_list.update(add_to_list) return contain_list def _execute_heartbeat(self): thread = threading.Thread(target=self._heartbeat_worker) thread.daemon = True thread.start() def _heartbeat_worker(self): while True: self._pub.publish('flexbe/heartbeat', Empty()) time.sleep(1) # sec class _attr_dict(dict): __getattr__ = dict.__getitem__ def _convert_dict(self, o): if isinstance(o, list): return [self._convert_dict(e) for e in o] elif isinstance(o, dict): return self._attr_dict( (k, self._convert_dict(v)) for k, v in o.items()) else: return o
class FlexbeOnboard(object): """ Controls the execution of robot behaviors. """ def __init__(self): self.be = None Logger.initialize() self._tracked_imports = list() # prepare temp folder self._tmp_folder = tempfile.mkdtemp() sys.path.append(self._tmp_folder) rospy.on_shutdown(self._cleanup_tempdir) # prepare manifest folder access self._behavior_lib = BehaviorLibrary() # prepare communication self.status_topic = 'flexbe/status' self.feedback_topic = 'flexbe/command_feedback' self._pub = ProxyPublisher({ self.feedback_topic: CommandFeedback, 'flexbe/heartbeat': Empty }) self._pub.createPublisher(self.status_topic, BEStatus, _latch=True) self._execute_heartbeat() # listen for new behavior to start self._enable_clear_imports = rospy.get_param('~enable_clear_imports', False) self._running = False self._run_lock = threading.Lock() self._switching = False self._switch_lock = threading.Lock() self._sub = ProxySubscriberCached() self._sub.subscribe('flexbe/start_behavior', BehaviorSelection, self._behavior_callback) rospy.sleep(0.5) # wait for publishers etc to really be set up self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') def _behavior_callback(self, msg): thread = threading.Thread(target=self._behavior_execution, args=[msg]) thread.daemon = True thread.start() # =================== # # Main execution loop # # ------------------- # def _behavior_execution(self, msg): # sending a behavior while one is already running is considered as switching if self._running: Logger.loginfo('--> Initiating behavior switch...') self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['received'])) else: Logger.loginfo('--> Starting new behavior...') # construct the behavior that should be executed be = self._prepare_behavior(msg) if be is None: Logger.logerr( 'Dropped behavior start request because preparation failed.') if self._running: self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) else: rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') return # perform the behavior switch if required with self._switch_lock: self._switching = True if self._running: self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['start'])) # ensure that switching is possible if not self._is_switchable(be): Logger.logerr( 'Dropped behavior start request because switching is not possible.' ) self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable'])) return # wait if running behavior is currently starting or stopping rate = rospy.Rate(100) while not rospy.is_shutdown(): active_state = self.be.get_current_state() if active_state is not None or not self._running: break rate.sleep() # extract the active state if any if active_state is not None: rospy.loginfo("Current state %s is kept active.", active_state.name) try: be.prepare_for_switch(active_state) self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['prepared'])) except Exception as e: Logger.logerr( 'Failed to prepare behavior switch:\n%s' % str(e)) self._pub.publish( self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) return # stop the rest rospy.loginfo('Preempting current behavior version...') self.be.preempt() # execute the behavior with self._run_lock: self._switching = False self.be = be self._running = True result = None try: rospy.loginfo('Behavior ready, execution starts now.') rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum) self.be.confirm() args = [self.be.requested_state_path ] if self.be.requested_state_path is not None else [] self._pub.publish( self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args)) result = self.be.execute() if self._switching: self._pub.publish( self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING)) else: self._pub.publish( self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED, args=[str(result)])) except Exception as e: self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED)) Logger.logerr('Behavior execution failed!\n%s' % str(e)) import traceback Logger.loginfo(traceback.format_exc()) result = result or "exception" # only set result if not executed # done, remove left-overs like the temporary behavior file try: # do not clear imports for now, not working correctly (e.g., flexbe/flexbe_app#66) # only if specifically enabled if not self._switching and self._enable_clear_imports: self._clear_imports() self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) if not self._switching: Logger.loginfo('Behavior execution finished with result %s.', str(result)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') self._running = False self.be = None # ==================================== # # Preparation of new behavior requests # # ------------------------------------ # def _prepare_behavior(self, msg): # get sourcecode from ros package try: behavior = self._behavior_lib.get_behavior(msg.behavior_id) if behavior is None: raise ValueError(msg.behavior_id) be_filepath = self._behavior_lib.get_sourcecode_filepath( msg.behavior_id, add_tmp=True) if os.path.isfile(be_filepath): be_file = open(be_filepath, "r") rospy.logwarn( "Found a tmp version of the referred behavior! Assuming local test run." ) else: be_filepath = self._behavior_lib.get_sourcecode_filepath( msg.behavior_id) be_file = open(be_filepath, "r") try: be_content = be_file.read() finally: be_file.close() except Exception as e: Logger.logerr('Failed to retrieve behavior from library:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # apply modifications if any try: file_content = "" last_index = 0 for mod in msg.modifications: file_content += be_content[last_index:mod. index_begin] + mod.new_content last_index = mod.index_end file_content += be_content[last_index:] if zlib.adler32(file_content.encode() ) & 0x7fffffff != msg.behavior_checksum: mismatch_msg = ( "Checksum mismatch of behavior versions! \n" "Attempted to load behavior: %s\n" "Make sure that all computers are on the same version a.\n" "Also try: rosrun flexbe_widget clear_cache" % str(be_filepath)) raise Exception(mismatch_msg) else: rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications)) except Exception as e: Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # create temp file for behavior class try: file_path = os.path.join(self._tmp_folder, 'tmp_%d.py' % msg.behavior_checksum) with open(file_path, "w") as sc_file: sc_file.write(file_content) except Exception as e: Logger.logerr( 'Failed to create temporary file for behavior class:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # import temp class file and initialize behavior try: with self._track_imports(): package = __import__( "tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum]) clsmembers = inspect.getmembers( package, lambda member: (inspect.isclass(member) and member .__module__ == package.__name__)) beclass = clsmembers[0][1] be = beclass() rospy.loginfo('Behavior ' + be.name + ' created.') except Exception as e: Logger.logerr('Exception caught in behavior definition:\n%s\n' 'See onboard terminal for more information.' % str(e)) import traceback traceback.print_exc() self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) if self._enable_clear_imports: self._clear_imports() return # initialize behavior parameters if len(msg.arg_keys) > 0: rospy.loginfo('The following parameters will be used:') try: for i in range(len(msg.arg_keys)): # action call has empty string as default, not a valid param key if msg.arg_keys[i] == '': continue found = be.set_parameter(msg.arg_keys[i], msg.arg_values[i]) if found: name_split = msg.arg_keys[i].rsplit('/', 1) behavior = name_split[0] if len(name_split) == 2 else '' key = name_split[-1] suffix = ' (' + behavior + ')' if behavior != '' else '' rospy.loginfo(key + ' = ' + msg.arg_values[i] + suffix) else: rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not defined') except Exception as e: Logger.logerr('Failed to initialize parameters:\n%s' % str(e)) self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # build state machine try: be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False) be.prepare_for_execution( self._convert_input_data(msg.input_keys, msg.input_values)) rospy.loginfo('State machine built.') except Exception as e: Logger.logerr('Behavior construction failed!\n%s\n' 'See onboard terminal for more information.' % str(e)) import traceback traceback.print_exc() self._pub.publish( self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) if self._enable_clear_imports: self._clear_imports() return return be # ================ # # Helper functions # # ---------------- # def _is_switchable(self, be): if self.be.name != be.name: Logger.logerr( 'Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s' % (self.be.name, be.name)) return False # locked inside # locked state exists in new behavior # ok, can switch return True def _cleanup_behavior(self, behavior_checksum): file_path = os.path.join(self._tmp_folder, 'tmp_%d.pyc' % behavior_checksum) try: os.remove(file_path) except OSError: pass try: os.remove(file_path + 'c') except OSError: pass def _clear_imports(self): for module in self._tracked_imports: if module in sys.modules: del sys.modules[module] self._tracked_imports = list() def _cleanup_tempdir(self): try: os.remove(self._tmp_folder) except OSError: pass def _convert_input_data(self, keys, values): result = dict() for k, v in zip(keys, values): # action call has empty string as default, not a valid input key if k == '': continue try: result[k] = self._convert_dict(cast(v)) except ValueError: # unquoted strings will raise a ValueError, so leave it as string in this case result[k] = str(v) except SyntaxError as se: Logger.loginfo( 'Unable to parse input value for key "%s", assuming string:\n%s\n%s' % (k, str(v), str(se))) result[k] = str(v) return result def _execute_heartbeat(self): thread = threading.Thread(target=self._heartbeat_worker) thread.daemon = True thread.start() def _heartbeat_worker(self): while True: self._pub.publish('flexbe/heartbeat', Empty()) time.sleep(1) def _convert_dict(self, o): if isinstance(o, list): return [self._convert_dict(e) for e in o] elif isinstance(o, dict): return self._attr_dict( (k, self._convert_dict(v)) for k, v in list(o.items())) else: return o class _attr_dict(dict): __getattr__ = dict.__getitem__ @contextlib.contextmanager def _track_imports(self): previous_modules = set(sys.modules.keys()) try: yield finally: self._tracked_imports.extend( set(sys.modules.keys()) - previous_modules)
class DrivepathTest(EventState): ''' Lets the robot move along a given path. ># path PoseStamped[] Array of Positions of the robot. ># speed float Speed of the robot <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. ''' def __init__(self): ''' Constructor ''' super(DrivepathTest, self).__init__(outcomes=['reached', 'failed'], input_keys = ['poses']) self._failed = False self._reached = False self._path = MoveBaseActionPath() self._pathTopic = '/controller/path' self._pub = ProxyPublisher({self._pathTopic: MoveBaseActionPath}) def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._reached: return 'reached' def on_enter(self, userdata): self._failed = False self._path.goal.target_path.poses = userdata.poses self._path.goal.target_path.header.frame_id = 'map' self._pub.publish(self._pathTopic, self._path) self._reached = True def on_stop(self): pass def on_exit(self, userdata): pass def on_pause(self): self._move_client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class VigirBehaviorMirror(object): ''' classdocs ''' def __init__(self): ''' Constructor ''' self._sm = None smach.set_loggers ( rospy.logdebug, # hide SMACH transition log spamming rospy.logwarn, rospy.logdebug, rospy.logerr ) # set up proxys for sm <--> GUI communication # publish topics self._pub = ProxyPublisher({'flexbe/behavior_update': String, 'flexbe/request_mirror_structure': Int32}) self._running = False self._stopping = False self._active_id = 0 self._starting_path = None self._current_struct = None self._outcome_topic = 'flexbe/mirror/outcome' self._struct_buffer = list() # listen for mirror message self._sub = ProxySubscriberCached() self._sub.subscribe(self._outcome_topic, UInt8) self._sub.enable_buffer(self._outcome_topic) self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback) self._sub.subscribe('flexbe/status', BEStatus, self._status_callback) self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback) self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback) def _mirror_callback(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn('Received a new mirror structure while mirror is already running, adding to buffer (ID: %s).' % str(msg.behavior_id)) elif self._active_id != 0 and msg.behavior_id != self._active_id: rospy.logwarn('ID of received mirror structure (%s) does not match expected ID (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id))) return else: rospy.loginfo('Received a new mirror structure for ID %s' % str(msg.behavior_id)) self._struct_buffer.append(msg) if self._active_id == msg.behavior_id: self._struct_buffer = list() self._mirror_state_machine(msg) rospy.loginfo('Mirror built.') self._execute_mirror() def _status_callback(self, msg): if msg.code == BEStatus.STARTED: thread = threading.Thread(target=self._start_mirror, args=[msg]) thread.daemon = True thread.start() else: thread = threading.Thread(target=self._stop_mirror, args=[msg]) thread.daemon = True thread.start() def _start_mirror(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn('Tried to start mirror while it is already running, will ignore.') return if len(msg.args) > 0: self._starting_path = "/" + msg.args[0][1:].replace("/", "_mirror/") + "_mirror" self._active_id = msg.behavior_id while self._sm is None and len(self._struct_buffer) > 0: struct = self._struct_buffer[0] self._struct_buffer = self._struct_buffer[1:] if struct.behavior_id == self._active_id: self._mirror_state_machine(struct) rospy.loginfo('Mirror built.') else: rospy.logwarn('Discarded mismatching buffered structure for ID %s' % str(struct.behavior_id)) if self._sm is None: rospy.logwarn('Missing correct mirror structure, requesting...') rospy.sleep(0.2) # no clean way to wait for publisher to be ready... self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id)) self._active_id = msg.behavior_id return self._execute_mirror() def _stop_mirror(self, msg): self._stopping = True if self._sm is not None and self._running: if msg is not None and msg.code == BEStatus.FINISHED: rospy.loginfo('Onboard behavior finished successfully.') self._pub.publish('flexbe/behavior_update', String()) elif msg is not None and msg.code == BEStatus.SWITCHING: self._starting_path = None rospy.loginfo('Onboard performing behavior switch.') elif msg is not None and msg.code == BEStatus.READY: rospy.loginfo('Onboard engine just started, stopping currently running mirror.') self._pub.publish('flexbe/behavior_update', String()) elif msg is not None: rospy.logwarn('Onboard behavior failed!') self._pub.publish('flexbe/behavior_update', String()) PreemptableState.preempt = True rate = rospy.Rate(10) while self._running: rate.sleep() else: rospy.loginfo('No onboard behavior is active.') self._active_id = 0 self._sm = None self._current_struct = None self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) if msg is not None and msg.code != BEStatus.SWITCHING: rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') self._stopping = False def _sync_callback(self, msg): if msg.behavior_id == self._active_id: thread = threading.Thread(target=self._restart_mirror, args=[msg]) thread.daemon = True thread.start() else: rospy.logerr('Cannot synchronize! Different behavior is running onboard, please stop execution!') thread = threading.Thread(target=self._stop_mirror, args=[None]) thread.daemon = True thread.start() def _restart_mirror(self, msg): rospy.loginfo('Restarting mirror for synchronization...') self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) if self._sm is not None and self._running: PreemptableState.preempt = True rate = rospy.Rate(10) while self._running: rate.sleep() self._sm = None if msg.current_state_checksum in self._state_checksums: current_state_path = self._state_checksums[msg.current_state_checksum] self._starting_path = "/" + current_state_path[1:].replace("/", "_mirror/") + "_mirror" rospy.loginfo('Current state: %s' % current_state_path) try: self._mirror_state_machine(self._current_struct) rospy.loginfo('Mirror built.') self._execute_mirror() except (AttributeError, smach.InvalidStateError): rospy.loginfo('Stopping synchronization because behavior has stopped.') def _execute_mirror(self): self._running = True rospy.loginfo("Executing mirror...") if self._starting_path is not None: LockableStateMachine.path_for_switch = self._starting_path rospy.loginfo("Starting mirror in state " + self._starting_path) self._starting_path = None result = 'preempted' try: result = self._sm.execute() except Exception as e: rospy.loginfo('Catched exception on preempt:\n%s' % str(e)) if self._sm is not None: self._sm.destroy() self._running = False rospy.loginfo('Mirror finished with result ' + result) def _mirror_state_machine(self, msg): self._current_struct = msg self._state_checksums = dict() root = None for con_msg in msg.containers: if con_msg.path.find('/') == -1: root = con_msg.path break self._add_node(msg, root) # calculate checksums of all states for con_msg in msg.containers: if con_msg.path.find('/') != -1: self._state_checksums[zlib.adler32(con_msg.path)] = con_msg.path def _add_node(self, msg, path): #rospy.loginfo('Add node: '+path) container = None for con_msg in msg.containers: if con_msg.path == path: container = con_msg break transitions = None if container.transitions is not None: transitions = {} for i in range(len(container.transitions)): transitions[container.outcomes[i]] = container.transitions[i] + '_mirror' path_frags = path.split('/') container_name = path_frags[len(path_frags)-1] if len(container.children) > 0: sm_outcomes = [] for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror') sm = JumpableStateMachine(outcomes=sm_outcomes) with sm: for child in container.children: self._add_node(msg, path+'/'+child) if len(transitions) > 0: container_transitions = {} for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[container.outcomes[i]] JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: self._sm = sm else: JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) def _jump_callback(self, msg): start_time = rospy.Time.now() current_elapsed = 0 jumpable = True while self._sm is None or not self._sm.is_running(): current_elapsed = rospy.Time.now() - start_time if current_elapsed > rospy.Duration.from_sec(10): jumpable = False break rospy.sleep(0.05) if jumpable: self._sm.jump_to(msg.stateName) else: rospy.logwarn('Could not jump in mirror: Mirror does not exist or is not running!') def _preempt_callback(self, msg): if self._sm is not None and self._sm.is_running(): rospy.logwarn('Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.') else: rospy.logwarn('Could not preempt mirror because it seems not to be running!')
class OperatableState(PreemptableState): """ A state that supports autonomy levels and silent mode. Also, it allows being tracked by a GUI or anything else as it reports each transition and its initial structure. An additional method is provided to report custom status messages to the widget. """ def __init__(self, *args, **kwargs): super(OperatableState, self).__init__(*args, **kwargs) self.transitions = None self.autonomy = None self._mute = False # is set to true when used in silent state machine (don't report transitions) self._last_requested_outcome = None self._outcome_topic = 'flexbe/mirror/outcome' self._request_topic = 'flexbe/outcome_request' self._debug_topic = 'flexbe/debug/current_state' self._pub = ProxyPublisher() self.__execute = self.execute self.execute = self._operatable_execute def _build_msg(self, prefix, msg): """ Adds this state to the initial structure message. @type prefix: string @param prefix: A path consisting of the container hierarchy containing this state. @type msg: ContainerStructure @param msg: The message that will finally contain the structure message. """ # set path name = prefix + self.name # no children children = None # set outcomes outcomes = self._outcome_list # set transitions and autonomy levels transitions = [] autonomy = [] for i in range(len(outcomes)): outcome = outcomes[i] if outcome == 'preempted': # set preempt transition transitions.append('preempted') autonomy.append(-1) else: transitions.append(str(self.transitions[outcome])) autonomy.append(self.autonomy[outcome]) # add to message msg.containers.append(Container(name, children, outcomes, transitions, autonomy)) def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: # reset previously requested outcome if applicable if self._last_requested_outcome is not None and outcome == OperatableState._loopback_name: self._pub.publish(self._request_topic, OutcomeRequest(outcome=255, target=self._parent._get_path() + "/" + self.name)) self._last_requested_outcome = None # request outcome because autonomy level is too low if not self._force_transition and (self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level or outcome != OperatableState._loopback_name and self._get_path() in rospy.get_param('/flexbe/breakpoints', [])): if outcome != self._last_requested_outcome: self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False) self._last_requested_outcome = outcome outcome = OperatableState._loopback_name # autonomy level is high enough, report the executed transition elif outcome != OperatableState._loopback_name: self._sent_outcome_requests = [] rospy.loginfo("State result: %s > %s", self.name, outcome) self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) self._pub.publish(self._debug_topic, String("%s > %s" % (self._get_path(), outcome))) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True) self._force_transition = False return outcome def _notify_skipped(self): super(OperatableState, self)._notify_skipped() def _enable_ros_control(self): super(OperatableState, self)._enable_ros_control() self._pub.createPublisher(self._outcome_topic, UInt8) self._pub.createPublisher(self._debug_topic, String) self._pub.createPublisher(self._request_topic, OutcomeRequest)
class LeapMotionMonitor(EventState): ''' Receive and classify LeapMotion messages. Can detect following situations: `no_object` detected, `still_object`, `moving_object`. -- leap_motion_topic string Leap Motion topic. -- exit_states string[] Stop monitoring and return outcome if detected situation in this list. -- pose_topic string Topic for publishing detected object pose for vizualizaton purpose (may be Empty). -- parameters float[] Parameteres: detection_period (s), position_tolerance (m), orientation_tolerance (rad). #> pose object Object pose before exit. <= no_object No object detected. `pose` key contains zero pose. <= still_object Object presents and it is not moving. `pose` key contains averaged pose. <= moving_object Object presents and it is moving. `pose` key contains last pose. ''' def __init__(self, leap_motion_topic, exit_states, pose_topic = '', parameters = [2.0, 0.02, 0.2]): super(LeapMotionMonitor, self).__init__(outcomes = ['no_object', 'still_object', 'moving_object'], output_keys = ['pose']) # store state parameter for later use. self._leap_topic = leap_motion_topic self._pose_topic = pose_topic self._exit_states = exit_states self._detecton_period = parameters[0] self._position_tolerance = parameters[1] self._orientation_tolerance = parameters[2] # setup proxies self._leap_subscriber = ProxySubscriberCached({ self._leap_topic: leapros }) if self._pose_topic: self._pose_publisher = ProxyPublisher({ self._pose_topic: PoseStamped }) else: self._pose_publisher = None # pose buffers self._pose = None self._pose_prev = None self._pose_averaged = None self._nonstillness_stamp = None def on_enter(self, userdata): # reset pose buffers self._pose = None self._pose_prev = None self._pose_averaged = None self._nonstillness_stamp = None Logger.loginfo('LeapMotionMonitor: start monitoring.') def execute(self, userdata): # check if new message is available if self._leap_subscriber.has_msg(self._leap_topic): # GET LEAP MOTION message leap_msg = self._leap_subscriber.get_last_msg(self._leap_topic) #Logger.loginfo('LeapMotionMonitor: leap_motion: \n%s' % str(leap_msg.palmpos)) # STORE POSE IN BUFFER self._pose = PoseVectorized() self._pose.header = Header(frame_id = 'leap_motion', stamp = rospy.Time.now()) self._pose.position = numpy.array([leap_msg.palmpos.x, leap_msg.palmpos.y, leap_msg.palmpos.z]) / 1000.0 # get axis directions from leap_msg # TODO use quaternion_from_rxyx axis_z = - numpy.array([leap_msg.normal.x, leap_msg.normal.y,leap_msg.normal.z, 0]) axis_y = - numpy.array([leap_msg.direction.x, leap_msg.direction.y,leap_msg.direction.z, 0]) axis_x = numpy.cross(axis_y[:3], axis_z[:3]) axis_x = numpy.append(axis_x, 0) # construct transformation matrix mat = numpy.matrix([ axis_x, axis_y, axis_z, numpy.array([0, 0, 0, 1]) ]).transpose() #Logger.loginfo('mat: \n%s' % str(mat)) # obtain quaternion self._pose.orientation = quaternion_from_matrix(mat) # PUBLISH POSE if self._pose_topic: self._pose_publisher.publish(self._pose_topic, self._pose.toPoseStamped()) # CHECK FOR FIRST CALL if self._pose_prev == None: self._pose_prev = PoseVectorized(self._pose) self._pose_averaged = PoseVectorized(self._pose) self._nonstillness_stamp = rospy.Time.now() return None # POSE AVERAGING self._pose_averaged.average_exp(self._pose, self._detecton_period) # CHECK FOR NO_OBJECT if PoseVectorized.eq(self._pose, self._pose_prev): # messages are identical, now check how long they are in this state if (self._pose.header.stamp - self._pose_prev.header.stamp).to_sec() > self._detecton_period: # NO_OBJECT is detected if 'no_object' in self._exit_states: Logger.loginfo('LeapMotionMonitor: no_object state is detected.') return 'no_object' # wait until detection_period return None else: self._pose_prev = self._pose # CHECK FOR STILL_OBJECT if PoseVectorized.eq_approx(self._pose, self._pose_averaged, self._position_tolerance, self._orientation_tolerance): # check if averaging is performed long enought if (self._pose.header.stamp - self._nonstillness_stamp).to_sec() > self._detecton_period: # STILL_OBJECT is_detected if 'still_object' in self._exit_states: Logger.loginfo('LeapMotionMonitor: still_object state is detected.') return 'still_object' # wait for detection_period from the start return None else: self._nonstillness_stamp = rospy.Time.now() # DEFAULT: MOVING OBJECT if 'moving_object' in self._exit_states: Logger.loginfo('LeapMotionMonitor: moving_object state is detected.') return 'moving_object' else: return None def on_exit(self, userdata): userdata.pose = self._pose.toPoseStamped()
class OperatableState(PreemptableState): """ A state that supports autonomy levels and silent mode. Also, it allows being tracked by a GUI or anything else as it reports each transition and its initial structure. An additional method is provided to report custom status messages to the widget. """ def __init__(self, *args, **kwargs): super(OperatableState, self).__init__(*args, **kwargs) self.transitions = None self.autonomy = None self._mute = False # is set to true when used in silent state machine (don't report transitions) self._sent_outcome_requests = [] # contains those outcomes that already requested a transition self._outcome_topic = '/flexbe/mirror/outcome' self._request_topic = '/flexbe/outcome_request' self._pub = ProxyPublisher() self.__execute = self.execute self.execute = self._operatable_execute def _build_msg(self, prefix, msg): """ Adds this state to the initial structure message. @type prefix: string @param prefix: A path consisting of the container hierarchy containing this state. @type msg: ContainerStructure @param msg: The message that will finally contain the structure message. """ # set path name = prefix + self.name # no children children = None # set outcomes outcomes = self._outcome_list # set transitions and autonomy levels transitions = [] autonomy = [] for i in range(len(outcomes)): outcome = outcomes[i] if outcome == 'preempted': # set preempt transition transitions.append('preempted') autonomy.append(-1) else: transitions.append(str(self.transitions[outcome])) autonomy.append(self.autonomy[outcome]) # add to message msg.containers.append(Container(name, children, outcomes, transitions, autonomy)) def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: log_requested_outcome = outcome # request outcome because autonomy level is too low if not self._force_transition and (self.autonomy.has_key(outcome) and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level): if self._sent_outcome_requests.count(outcome) == 0: self._pub.publish(self._request_topic, OutcomeRequest(outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False) self._sent_outcome_requests.append(outcome) outcome = OperatableState._loopback_name # autonomy level is high enough, report the executed transition elif outcome != OperatableState._loopback_name: self._sent_outcome_requests = [] rospy.loginfo("State result: %s > %s", self.name, outcome) self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True) self._force_transition = False return outcome def _enable_ros_control(self): super(OperatableState, self)._enable_ros_control() self._pub.createPublisher(self._outcome_topic, UInt8) self._pub.createPublisher(self._request_topic, OutcomeRequest)
class VigirBehaviorMirror(object): ''' classdocs ''' def __init__(self): ''' Constructor ''' self._sm = None smach.set_loggers( rospy.logdebug, # hide SMACH transition log spamming rospy.logwarn, rospy.logdebug, rospy.logerr) # set up proxys for sm <--> GUI communication # publish topics self._pub = ProxyPublisher({ 'flexbe/behavior_update': String, 'flexbe/request_mirror_structure': Int32 }) self._running = False self._stopping = False self._active_id = 0 self._starting_path = None self._current_struct = None self._sync_lock = threading.Lock() self._outcome_topic = 'flexbe/mirror/outcome' self._struct_buffer = list() # listen for mirror message self._sub = ProxySubscriberCached() self._sub.subscribe(self._outcome_topic, UInt8) self._sub.enable_buffer(self._outcome_topic) self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_callback) self._sub.subscribe('flexbe/status', BEStatus, self._status_callback) self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback) self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback) def _mirror_callback(self, msg): rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn( 'Received a new mirror structure while mirror is already running, adding to buffer (checksum: %s).' % str(msg.behavior_id)) elif self._active_id != 0 and msg.behavior_id != self._active_id: rospy.logwarn( 'checksum of received mirror structure (%s) does not match expected checksum (%s), will ignore.' % (str(msg.behavior_id), str(self._active_id))) return else: rospy.loginfo('Received a new mirror structure for checksum %s' % str(msg.behavior_id)) self._struct_buffer.append(msg) if self._active_id == msg.behavior_id: self._struct_buffer = list() self._mirror_state_machine(msg) rospy.loginfo('Mirror built.') self._execute_mirror() def _status_callback(self, msg): if msg.code == BEStatus.STARTED: thread = threading.Thread(target=self._start_mirror, args=[msg]) thread.daemon = True thread.start() else: thread = threading.Thread(target=self._stop_mirror, args=[msg]) thread.daemon = True thread.start() def _start_mirror(self, msg): with self._sync_lock: rate = rospy.Rate(10) while self._stopping: rate.sleep() if self._running: rospy.logwarn( 'Tried to start mirror while it is already running, will ignore.' ) return if len(msg.args) > 0: self._starting_path = "/" + msg.args[0][1:].replace( "/", "_mirror/") + "_mirror" self._active_id = msg.behavior_id while self._sm is None and len(self._struct_buffer) > 0: struct = self._struct_buffer[0] self._struct_buffer = self._struct_buffer[1:] if struct.behavior_id == self._active_id: self._mirror_state_machine(struct) rospy.loginfo('Mirror built.') else: rospy.logwarn( 'Discarded mismatching buffered structure for checksum %s' % str(struct.behavior_id)) if self._sm is None: rospy.logwarn( 'Missing correct mirror structure, requesting...') rospy.sleep( 0.2 ) # no clean wayacquire to wait for publisher to be ready... self._pub.publish('flexbe/request_mirror_structure', Int32(msg.behavior_id)) self._active_id = msg.behavior_id return self._execute_mirror() def _stop_mirror(self, msg): with self._sync_lock: self._stopping = True if self._sm is not None and self._running: if msg is not None and msg.code == BEStatus.FINISHED: rospy.loginfo('Onboard behavior finished successfully.') self._pub.publish('flexbe/behavior_update', String()) elif msg is not None and msg.code == BEStatus.SWITCHING: self._starting_path = None rospy.loginfo('Onboard performing behavior switch.') elif msg is not None and msg.code == BEStatus.READY: rospy.loginfo( 'Onboard engine just started, stopping currently running mirror.' ) self._pub.publish('flexbe/behavior_update', String()) elif msg is not None: rospy.logwarn('Onboard behavior failed!') self._pub.publish('flexbe/behavior_update', String()) PreemptableState.preempt = True rate = rospy.Rate(10) while self._running: rate.sleep() else: rospy.loginfo('No onboard behavior is active.') self._active_id = 0 self._sm = None self._current_struct = None self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) if msg is not None and msg.code != BEStatus.SWITCHING: rospy.loginfo('\033[92m--- Behavior Mirror ready! ---\033[0m') self._stopping = False def _sync_callback(self, msg): if msg.behavior_id == self._active_id: thread = threading.Thread(target=self._restart_mirror, args=[msg]) thread.daemon = True thread.start() else: rospy.logerr( 'Cannot synchronize! Different behavior is running onboard, please stop execution!' ) thread = threading.Thread(target=self._stop_mirror, args=[None]) thread.daemon = True thread.start() def _restart_mirror(self, msg): with self._sync_lock: rospy.loginfo('Restarting mirror for synchronization...') self._sub.remove_last_msg(self._outcome_topic, clear_buffer=True) if self._sm is not None and self._running: PreemptableState.preempt = True rate = rospy.Rate(10) while self._running: rate.sleep() self._sm = None if msg.current_state_checksum in self._state_checksums: current_state_path = self._state_checksums[ msg.current_state_checksum] self._starting_path = "/" + current_state_path[1:].replace( "/", "_mirror/") + "_mirror" rospy.loginfo('Current state: %s' % current_state_path) try: self._mirror_state_machine(self._current_struct) rospy.loginfo('Mirror built.') except (AttributeError, smach.InvalidStateError): rospy.loginfo( 'Stopping synchronization because behavior has stopped.') self._execute_mirror() def _execute_mirror(self): self._running = True rospy.loginfo("Executing mirror...") if self._starting_path is not None: LockableStateMachine.path_for_switch = self._starting_path rospy.loginfo("Starting mirror in state " + self._starting_path) self._starting_path = None result = 'preempted' try: result = self._sm.execute() except Exception as e: rospy.loginfo('Caught exception on preempt:\n%s' % str(e)) result = 'preempted' if self._sm is not None: self._sm.destroy() self._running = False rospy.loginfo('Mirror finished with result ' + result) def _mirror_state_machine(self, msg): self._current_struct = msg self._state_checksums = dict() root = None for con_msg in msg.containers: if con_msg.path.find('/') == -1: root = con_msg.path break self._add_node(msg, root) # calculate checksums of all states for con_msg in msg.containers: if con_msg.path.find('/') != -1: self._state_checksums[zlib.adler32( con_msg.path)] = con_msg.path def _add_node(self, msg, path): #rospy.loginfo('Add node: '+path) container = None for con_msg in msg.containers: if con_msg.path == path: container = con_msg break transitions = None if container.transitions is not None: transitions = {} for i in range(len(container.transitions)): transitions[container. outcomes[i]] = container.transitions[i] + '_mirror' path_frags = path.split('/') container_name = path_frags[len(path_frags) - 1] if len(container.children) > 0: sm_outcomes = [] for outcome in container.outcomes: sm_outcomes.append(outcome + '_mirror') sm = JumpableStateMachine(outcomes=sm_outcomes) with sm: for child in container.children: self._add_node(msg, path + '/' + child) if len(transitions) > 0: container_transitions = {} for i in range(len(container.transitions)): container_transitions[sm_outcomes[i]] = transitions[ container.outcomes[i]] JumpableStateMachine.add(container_name + '_mirror', sm, transitions=container_transitions) else: self._sm = sm else: JumpableStateMachine.add(container_name + '_mirror', MirrorState(container_name, path, container.outcomes, container.autonomy), transitions=transitions) def _jump_callback(self, msg): start_time = rospy.Time.now() current_elapsed = 0 jumpable = True while self._sm is None or not self._sm.is_running(): current_elapsed = rospy.Time.now() - start_time if current_elapsed > rospy.Duration.from_sec(10): jumpable = False break rospy.sleep(0.05) if jumpable: self._sm.jump_to(msg.stateName) else: rospy.logwarn( 'Could not jump in mirror: Mirror does not exist or is not running!' ) def _preempt_callback(self, msg): if self._sm is not None and self._sm.is_running(): rospy.logwarn( 'Explicit preempting is currently ignored, mirror should be preempted by onboard behavior.' ) else: rospy.logwarn( 'Could not preempt mirror because it seems not to be running!')
class OperatableStateMachine(PreemptableStateMachine): """ A state machine that can be operated. It synchronizes its current state with the mirror and supports some control mechanisms. """ autonomy_level = 3 silent_mode = False def __init__(self, *args, **kwargs): super(OperatableStateMachine, self).__init__(*args, **kwargs) self._message = None self._rate = rospy.Rate(10) self._do_rate_sleep = True self.id = None self.autonomy = None self._autonomy = {} self._ordered_states = [] self._inner_sync_request = False self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def execute(self, parent_ud=smach.UserData()): # First time in this state machine if self._current_state is None: # Spew some info smach.loginfo( "State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) # Set initial state self._set_current_state(self._initial_state_label) # Initialize preempt state self._preempted_label = None self._preempted_state = None # Call start callbacks self.call_start_cbs() # Copy input keys self._copy_input_keys(parent_ud, self.userdata) container_outcome = self._loopback_name if self.id is not None: # only top-level statemachine should loop for outcome while container_outcome == self._loopback_name and not smach.is_shutdown( ): # Update the state machine container_outcome = self._async_execute(parent_ud) else: container_outcome = self._async_execute(parent_ud) if smach.is_shutdown(): container_outcome = 'preempted' return container_outcome def _async_execute(self, parent_ud=smach.UserData()): """Run the state machine on entry to this state. This will set the "closed" flag and spin up the execute thread. Once this flag has been set, it will prevent more states from being added to the state machine. """ # This will prevent preempts from getting propagated to non-existent children with self._state_transitioning_lock: # Check state consistency try: self.check_consistency() except (smach.InvalidStateError, smach.InvalidTransitionError): smach.logerr("Container consistency check failed.") return None # Set running flag self._is_running = True # Initialize container outcome container_outcome = None # Step through state machine if self._is_running and not smach.is_shutdown(): # Update the state machine container_outcome = self._update_once() if self._do_rate_sleep and self._current_state is not None and not isinstance( self._current_state, OperatableStateMachine): try: # sleep with the rate of the state and update container rate accordingly self._rate = self._current_state._rate self._rate.sleep() except ROSInterruptException: rospy.logwarn('Interrupted rate sleep.') if container_outcome is not None and container_outcome != self._loopback_name: # Copy output keys self._copy_output_keys(self.userdata, parent_ud) else: container_outcome = self._loopback_name # provide explicit sync as back-up functionality # should be used only if there is no other choice # since it requires additional 8 byte + header update bandwith and time to restart mirror if self._inner_sync_request and self._get_deep_state() is not None: self._inner_sync_request = False if self.id is None: self._parent._inner_sync_request = True else: msg = BehaviorSync() msg.behavior_id = self.id msg.current_state_checksum = zlib.adler32( self._get_deep_state()._get_path()) self._pub.publish('flexbe/mirror/sync', msg) # We're no longer running self._is_running = False return container_outcome @staticmethod def add(label, state, transitions=None, autonomy=None, remapping=None): """ Add a state to the opened state machine. @type label: string @param label: The label of the state being added. @param state: An instance of a class implementing the L{State} interface. @param transitions: A dictionary mapping state outcomes to other state labels or container outcomes. @param autonomy: A dictionary mapping state outcomes to their required autonomy level @param remapping: A dictionary mapping local userdata keys to userdata keys in the container. """ self = StateMachine._currently_opened_container() # add loopback transition to loopback states if isinstance(state, LoopbackState): transitions[LoopbackState._loopback_name] = label autonomy[LoopbackState._loopback_name] = -1 if isinstance(state, OperatableStateMachine): transitions[OperatableStateMachine._loopback_name] = label autonomy[OperatableStateMachine._loopback_name] = -1 self._ordered_states.append(state) state.name = label state.transitions = transitions state.autonomy = autonomy state._parent = self StateMachine.add(label, state, transitions, remapping) self._autonomy[label] = autonomy def replace(self, new_state): old_state = self._states[new_state.name] new_state.transitions = old_state.transitions new_state.autonomy = old_state.autonomy new_state._parent = old_state._parent self._ordered_states[self._ordered_states.index(old_state)] = new_state self._states[new_state.name] = new_state def destroy(self): self._notify_stop() self._disable_ros_control() self._sub.unsubscribe_topic('flexbe/command/autonomy') self._sub.unsubscribe_topic('flexbe/command/sync') self._sub.unsubscribe_topic('flexbe/command/attach') self._sub.unsubscribe_topic('flexbe/request_mirror_structure') StateLogger.shutdown() def confirm(self, name, id): """ Confirms the state machine and triggers the creation of the structural message. It is mandatory to call this function at the top-level state machine between building it and starting its execution. @type name: string @param name: The name of this state machine to identify it. """ self.name = name self.id = id self._pub.createPublisher( 'flexbe/mirror/sync', BehaviorSync, _latch=True ) # Update mirror with currently active state (high bandwidth mode) self._pub.createPublisher('flexbe/mirror/preempt', Empty, _latch=True) # Preempts the mirror self._pub.createPublisher( 'flexbe/mirror/structure', ContainerStructure) # Sends the current structure to the mirror self._pub.createPublisher('flexbe/log', BehaviorLog) # Topic for logs to the GUI self._pub.createPublisher( 'flexbe/command_feedback', CommandFeedback ) # Gives feedback about executed commands to the GUI self._sub.subscribe('flexbe/command/autonomy', UInt8, self._set_autonomy_level) self._sub.subscribe('flexbe/command/sync', Empty, self._sync_callback) self._sub.subscribe('flexbe/command/attach', UInt8, self._attach_callback) self._sub.subscribe('flexbe/request_mirror_structure', Int32, self._mirror_structure_callback) StateLogger.initialize(name) if OperatableStateMachine.autonomy_level != 255: self._enable_ros_control() rospy.sleep(0.5) # no clean way to wait for publisher to be ready... self._notify_start() def _set_autonomy_level(self, msg): """ Sets the current autonomy level. """ if OperatableStateMachine.autonomy_level != msg.data: rospy.loginfo('--> Autonomy changed to %d', msg.data) if msg.data < 0: self.preempt() else: OperatableStateMachine.autonomy_level = msg.data self._pub.publish('flexbe/command_feedback', CommandFeedback(command="autonomy", args=[])) def _sync_callback(self, msg): rospy.loginfo("--> Synchronization requested...") msg = BehaviorSync() msg.behavior_id = self.id while self._get_deep_state() is None: rospy.sleep(0.1) msg.current_state_checksum = zlib.adler32( self._get_deep_state()._get_path()) self._pub.publish('flexbe/mirror/sync', msg) self._pub.publish('flexbe/command_feedback', CommandFeedback(command="sync", args=[])) rospy.loginfo("<-- Sent synchronization message for mirror.") def _attach_callback(self, msg): rospy.loginfo("--> Enabling control...") # set autonomy level OperatableStateMachine.autonomy_level = msg.data # enable control of states self._enable_ros_control() self._inner_sync_request = True # send command feedback cfb = CommandFeedback(command="attach") cfb.args.append(self.name) self._pub.publish('flexbe/command_feedback', cfb) rospy.loginfo("<-- Sent attach confirm.") def _mirror_structure_callback(self, msg): rospy.loginfo("--> Creating behavior structure for mirror...") msg = self._build_msg('') msg.behavior_id = self.id self._pub.publish('flexbe/mirror/structure', msg) rospy.loginfo("<-- Sent behavior structure for mirror.") def _transition_allowed(self, label, outcome): return self._autonomy[label][ outcome] < OperatableStateMachine.autonomy_level def _build_msg(self, prefix, msg=None): """ Adds this state machine to the initial structure message. @type prefix: string @param prefix: A path consisting of the container hierarchy containing this state. @type msg: ContainerStructure @param msg: The message that will finally contain the structure message. """ # set children children = [] for state in self._ordered_states: children.append(str(state.name)) # set name name = prefix + (self.name if self.id is None else '') if msg is None: # top-level state machine (has no transitions) self._message = ContainerStructure() outcomes = list(self._outcomes) transitions = None autonomy = None else: # lower-level state machine self._message = msg outcomes = list(self.transitions) # set transitions and autonomy transitions = [] autonomy = [] for i in range(len(self.transitions)): outcome = outcomes[i] if outcome == 'preempted': # set preempt transition transitions.append('preempted') autonomy.append(-1) else: transitions.append(str(self.transitions[outcome])) autonomy.append(self.autonomy[outcome]) # add to message self._message.containers.append( Container(name, children, outcomes, transitions, autonomy)) # build message for children for state in self._ordered_states: state._build_msg(name + '/', self._message) # top-level state machine returns the message if msg is None: return self._message def _notify_start(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_start() if isinstance(state, OperatableStateMachine): state._notify_start() def _enable_ros_control(self): self._is_controlled = True for state in self._ordered_states: if isinstance(state, LoopbackState): state._enable_ros_control() if isinstance(state, OperatableStateMachine): state._enable_ros_control() def _notify_stop(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_stop() state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._notify_stop() def _disable_ros_control(self): self._is_controlled = False for state in self._ordered_states: if isinstance(state, LoopbackState): state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._disable_ros_control() def on_exit(self, userdata): if self._current_state is not None: ud = smach.Remapper( self.userdata, self._current_state.get_registered_input_keys(), self._current_state.get_registered_output_keys(), self._remappings[self._current_state.name]) self._current_state._entering = True self._current_state.on_exit(ud) self._current_state = None
class SetJointStateBase(Dummy): ''' Base class for states which move robot to named pose using FollowJointState controller. Pose is loaded from binary parameter from Parameter Server as JointState message. Then state activate FollowJointState controller and publish pose. Movement is considered finished when position error is less then given tolerance. -- controller string FollowJointState controller namespace. -- tolerance float Position tolerance (rad). -- timeout float Movement timeout (s). -- joint_topic string Topic where actual pose published. <= done Finished. <= failed Failed to activate FollowJointState controller. <= timeout Timeout reached. ''' def __init__(self, controller='motion/controller/joint_state_head', tolerance=0.17, timeout=10.0, joint_topic="joint_states", outcomes=['done', 'failed', 'timeout']): super(SetJointStateBase, self).__init__(outcomes=outcomes) # Store topic parameter for later use. self._controller = controller self._joint_topic = joint_topic self._tolerance = tolerance self._timeout = Duration.from_sec(timeout) # create proxies self._action_client = ProxyActionClient( {self._controller: SetOperationalAction}) self._pose_publisher = ProxyPublisher( {self._controller + '/in_joints_ref': JointState}) self._pose_subscriber = ProxySubscriberCached( {self._joint_topic: JointState}) # timestamp self._timestamp = None # error in enter hook self._error = False def load_joint_state_msg(self, pose_ns, pose_param): # derive parameter full name if pose_ns: pose_param = pose_ns + '/' + pose_param # Load JointState message from Parameter Server try: goal_raw = rospy.get_param(pose_param) except KeyError as e: raise KeyError, "SetJointStateBase: Unable to get '" + pose_param + "' parameter." if not isinstance(goal_raw, xmlrpclib.Binary): raise TypeError, "SetJointStateBase: ROS parameter '" + pose_param + "' is not a binary data." # deserialize self._target_joint_state = JointState() self._target_joint_state.deserialize(goal_raw.data) # create joint index to simplify tolerance check self._joint_target_pose = { name: position for name, position in izip(self._target_joint_state.name, self._target_joint_state.position) } def on_enter(self, userdata): self._error = False # activate controller actiavtion_request = SetOperationalGoal() actiavtion_request.operational = True actiavtion_request.resources = self._target_joint_state.name try: self._action_client.send_goal(self._controller, actiavtion_request) except Exception as e: Logger.logwarn( 'SetJointStateBase: Failed to send the SetOperational command:\n%s' % str(e)) self._error = True return # set start timestamp self._timestamp = Time.now() def execute(self, userdata): # error in start hook if self._error: return 'failed' # check if controller is active if not self._action_client.is_active(self._controller): Logger.loginfo( 'SetJointStateBase: controller was deactivated by external cause.' ) return 'failed' # check if time elasped if Time.now() - self._timestamp > self._timeout: Logger.loginfo('SetJointStateBase: timeout.') return 'timeout' # publish goal pose self._pose_publisher.publish(self._controller + '/in_joints_ref', self._target_joint_state) # check tolerance joints_msg = self._pose_subscriber.get_last_msg(self._joint_topic) on_position = True for name, pos in izip(joints_msg.name, joints_msg.position): target_pos = self._joint_target_pose.get(name) if (target_pos != None): if abs(target_pos - pos) > self._tolerance: on_position = False break if on_position: Logger.loginfo('SetJointStateBase: on position') return 'done' def on_exit(self, userdata): if self._action_client.is_active(self._controller): try: self._action_client.cancel(self._controller) except Exception as e: Logger.logwarn('SetJointStateBase: failed to deactivate `' + self._controller + '` controller:\n%s' % str(e))
class TurnState(EventState): ''' Turns the robot in place to a set angle and with set speed. Uses /odom data. -- angle float Angle to turn to in degrees, +/- for direction. -- speed float Turn speed <= failed If behavior is unable to ready on time <= done Example for a failure outcome. ''' def __init__(self, angle, speed): super(TurnState, self).__init__(outcomes=['failed', 'done']) self.odom_data = None self.initial_orientation = None self.cur_orientation = None self._angle = angle self._speed = speed self._turn = False self._turn_angle = None self.cmd_pub = Twist() self._vel_topic = '/cmd_vel' self._odom_topic = '/odom' #create publisher passing it the vel_topic_name and msg_type self.pub_cmd_vel = ProxyPublisher({self._vel_topic: Twist}) #create subscriber self.sub_odom = ProxySubscriberCached({self._odom_topic: Odometry}) def execute(self, userdata): if self.sub_odom.has_msg(self._odom_topic): self.data = self.sub_odom.get_last_msg(self._odom_topic) self.sub_odom.remove_last_msg(self._odom_topic) # Logger.loginfo('New Current orientation from Odom data.pose.pose.orientation: %s' % self.data.pose.pose.orientation) self.cur_orientation = self.data.pose.pose.orientation cur_angle = transformations.euler_from_quaternion((self.cur_orientation.x, self.cur_orientation.y, self.cur_orientation.z, self.cur_orientation.w)) start_angle = transformations.euler_from_quaternion((self.initial_orientation.x, self.initial_orientation.y, self.initial_orientation.z, self.initial_orientation.w)) # Logger.loginfo('initial orientation: %f' % start_angle[2]) # Logger.loginfo('current orientation: %f' % cur_angle[2]) turned_so_far = math.fabs(cur_angle[2] - start_angle[2]) # Logger.loginfo ("The Robot turned so far: %s" % turned_so_far) if (turned_so_far >= self._turn_angle): # Turn completed self._turn = False Logger.loginfo ("Turn successfully completed!") return 'done' # Turn the robot if self._turn: self.cmd_pub.linear.x = 0.0 if self._angle > 0: self.cmd_pub.angular.z = self._speed else: self.cmd_pub.angular.z = -self._speed self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub) else: # Send stop command to the robot if turning is completed self.cmd_pub.angular.z = 0.0 self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub) def on_exit(self, userdata): self.cmd_pub.angular.z = 0.0 self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub) Logger.loginfo("Turn in place COMPLETED! Exiting the state!") # def on_start(self): def on_enter(self, userdata): Logger.loginfo("Turn in place READY!") # self._start_time = rospy.Time.now() #bug detected! (move to on_enter) if not self.pub_cmd_vel: Logger.loginfo('Failed to setup the publisher.') return 'failed' # Read current orientation from /odom if self.sub_odom.has_msg(self._odom_topic): self.data = self.sub_odom.get_last_msg(self._odom_topic) # Logger.loginfo('Set Initial Current Robot orientation: %s' % self.data.pose.pose.orientation) self.cur_orientation = self.data.pose.pose.orientation # TODO Seting publishing rate - should I do this in FlexBe???? r = rospy.Rate(30) # Convert the input turn angle value from degrees to Rad self._turn_angle = math.fabs((self._angle * math.pi)/ 180) Logger.loginfo ("Set TurnAngle _ angle to turn to in Rad: %s" % self._turn_angle) # Save the initial robot orientation in self.initial_orientation for latter use # if not self.initial_orientation: Logger.loginfo('Save the initial robot orientation ') self.initial_orientation = self.cur_orientation Logger.loginfo('Initial Robot orientation: %s' % self.initial_orientation) # Enable turning self._turn = True def on_stop(self): Logger.loginfo("Turn in place STOPPED!") self.initial_orientation = None self.cmd_pub.angular.z = 0.0 self.cmd_pub.linear.x = 0.0 self.pub_cmd_vel.publish(self._vel_topic, self.cmd_pub) def sub_odom_callback(self, data): self.data = data
class CreateWaypointState(EventState): ''' This state defines the starting point for a pure pursuit system.. -- target_frame: string Coordinate frame of target point (default: 'map') -- target_x: float Target point x-coordinate (m) -- target_y: float Target point y-coordinate (m) -- marker_topic: string topic of the RViz marker used for visualization (default: '/pure_pursuit_marker') -- marker_size: float Size of RViz marker used for target (default: 0.05) #> target_point object The target point (becomes next node's prior) <= done Reached the end of target relevance ''' def __init__(self, target_frame='map', target_x=1.0, target_y=0.1, marker_topic='/pure_pursuit_marker', marker_size=0.05): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(CreateWaypointState, self).__init__(outcomes=['done'], output_keys=['target_point']) # Store state parameter for later use. self._target = PointStamped() self._target.header.stamp = rospy.Time.now() self._target.header.frame_id = target_frame self._target.point = Point(target_x, target_y, 0.0) self._done = None # Track the outcome so we can detect if transition is blocked self._marker_topic = marker_topic self._marker_pub = ProxyPublisher({self._marker_topic: Marker}) self._marker = Marker() self._marker.header.frame_id = target_frame self._marker.header.stamp = rospy.Time.now() self._marker.ns = "pure_pursuit_waypoints" self._marker.id = int(target_x * 1000000) + int(target_y * 1000) self._marker.type = Marker.SPHERE self._marker.action = Marker.ADD self._marker.pose.position.x = target_x self._marker.pose.position.y = target_y self._marker.pose.position.z = 0.0 self._marker.pose.orientation.x = 0.0 self._marker.pose.orientation.y = 0.0 self._marker.pose.orientation.z = 0.0 self._marker.pose.orientation.w = 1.0 self._marker.scale.x = marker_size self._marker.scale.y = marker_size self._marker.scale.z = marker_size self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.0 self._marker.color.g = 1.0 self._marker.color.b = 1.0 # This is only to pass data along to the next state def execute(self, userdata): userdata.target_point = self._target # Set the user data for passing to next node return 'done' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._marker.action = Marker.MODIFY self._marker.color.a = 1.0 self._marker.color.r = 0.0 self._marker.color.g = 1.0 # Indicate this target is active self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) def on_exit(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.8 # Indicate this target is no longer active self._marker.color.g = 0.0 self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) def on_start(self): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._marker.action = Marker.ADD self._marker.color.a = 1.0 # Set alpha otherwise the marker is invisible self._marker.color.r = 0.0 self._marker.color.g = 1.0 self._marker.color.b = 1.0 # Indicate this target is planned self._marker_pub.publish(self._marker_topic, self._marker)
class VigirBeOnboard(object): ''' Implements an idle state where the robot waits for a behavior to be started. ''' def __init__(self): ''' Constructor ''' self.be = None Logger.initialize() #ProxyPublisher._simulate_delay = True #ProxySubscriberCached._simulate_delay = True # prepare temp folder rp = rospkg.RosPack() self._tmp_folder = os.path.join(rp.get_path('flexbe_onboard'), 'tmp/') if not os.path.exists(self._tmp_folder): os.makedirs(self._tmp_folder) sys.path.append(self._tmp_folder) # prepare manifest folder access manifest_folder = os.path.join(rp.get_path('flexbe_behaviors'), 'behaviors/') rospy.loginfo("Parsing available behaviors...") file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')] manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)]) self._behavior_lib = dict() for i in range(len(manifests)): m = ET.parse(manifests[i]).getroot() e = m.find("executable") self._behavior_lib[i] = {"name": m.get("name"), "package": e.get("package_path").split(".")[0], "file": e.get("package_path").split(".")[1], "class": e.get("class")} # rospy.loginfo("+++ " + self._behavior_lib[i]["name"]) self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() self.status_topic = '/flexbe/status' self.feedback_topic = '/flexbe/command_feedback' self._pub.createPublisher(self.status_topic, BEStatus, _latch = True) self._pub.createPublisher(self.feedback_topic, CommandFeedback) # listen for new behavior to start self._running = False self._switching = False self._sub.subscribe('/flexbe/start_behavior', BehaviorSelection, self._behavior_callback) # heartbeat self._pub.createPublisher('/flexbe/heartbeat', Empty) self._execute_heartbeat() rospy.sleep(0.5) # wait for publishers etc to really be set up self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') def _behavior_callback(self, msg): thread = threading.Thread(target=self._behavior_execution, args=[msg]) thread.daemon = True thread.start() def _behavior_execution(self, msg): if self._running: Logger.loginfo('--> Initiating behavior switch...') self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['received'])) else: Logger.loginfo('--> Starting new behavior...') be = self._prepare_behavior(msg) if be is None: Logger.logerr('Dropped behavior start request because preparation failed.') if self._running: self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) else: rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') return if self._running: if self._switching: Logger.logwarn('Already switching, dropped new start request.') return self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start'])) if not self._is_switchable(be): Logger.logerr('Dropped behavior start request because switching is not possible.') self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable'])) return self._switching = True active_state = self.be.get_current_state() rospy.loginfo("Current state %s is kept active.", active_state.name) try: be.prepare_for_switch(active_state) self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared'])) except Exception as e: Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e)) self._switching = False self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) return rospy.loginfo('Preempting current behavior version...') self.be.preempt_for_switch() rate = rospy.Rate(10) while self._running: rate.sleep() self._switching = False self._running = True self.be = be result = "" try: rospy.loginfo('Behavior ready, execution starts now.') rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum) self.be.confirm() args = [self.be.requested_state_path] if self.be.requested_state_path is not None else [] self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args)) result = self.be.execute() if self._switching: self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING)) else: self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED)) except Exception as e: self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED)) Logger.logerr('Behavior execution failed!\n%s' % str(e)) result = "failed" try: self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) self.be = None if not self._switching: rospy.loginfo('Behavior execution finished with result %s.', str(result)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') self._running = False def _prepare_behavior(self, msg): # get sourcecode from ros package try: rp = rospkg.RosPack() behavior = self._behavior_lib[msg.behavior_id] be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py') if os.path.isfile(be_filepath): be_file = open(be_filepath, "r") rospy.logwarn("Found a tmp version of the referred behavior! Assuming local test run.") else: be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py') be_file = open(be_filepath, "r") be_content = be_file.read() be_file.close() except Exception as e: Logger.logerr('Failed to retrieve behavior from library:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # apply modifications if any try: file_content = "" last_index = 0 for mod in msg.modifications: file_content += be_content[last_index:mod.index_begin] + mod.new_content last_index = mod.index_end file_content += be_content[last_index:] if zlib.adler32(file_content) != msg.behavior_checksum: mismatch_msg = ("checksum mismatch of behavior versions! \n" "Attempted to load behavior: %s" % str(be_filepath)) raise Exception(mismatch_msg) else: rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications)) except Exception as e: Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # create temp file for behavior class try: file_path = os.path.join(self._tmp_folder, 'tmp_%d.py' % msg.behavior_checksum) sc_file = open(file_path, "w") sc_file.write(file_content) sc_file.close() except Exception as e: Logger.logerr('Failed to create temporary file for behavior class:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # import temp class file and initialize behavior try: package = __import__("tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum]) clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__ == package.__name__) beclass = clsmembers[0][1] be = beclass() rospy.loginfo('Behavior ' + be.name + ' created.') except Exception as e: Logger.logerr('Unable to import temporary behavior file:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # import contained behaviors contain_list = {} try: contain_list = self._build_contains(be, "") except Exception as e: Logger.logerr('Failed to load contained behaviors:\n%s' % str(e)) return # initialize behavior parameters if len(msg.arg_keys) > 0: rospy.loginfo('The following parameters will be used:') try: for i in range(len(msg.arg_keys)): key_splitted = msg.arg_keys[i].rsplit('/', 1) behavior = key_splitted[0] key = key_splitted[1] found = False if behavior == '' and hasattr(be, key): self._set_typed_attribute(be, key, msg.arg_values[i]) found = True for b in contain_list: if hasattr(contain_list[b], key): self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b) found = True if not found: rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not implemented') except Exception as e: Logger.logerr('Failed to initialize parameters:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # build state machine try: be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False) be.prepare_for_execution() rospy.loginfo('State machine built.') except Exception as e: Logger.logerr('Behavior construction failed!\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return return be def _is_switchable(self, be): if self.be.name != be.name: Logger.logerr('Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s' % (self.be.name, be.name)) return False # locked inside # locked state exists in new behavior #if self.be.id == be.id: #Logger.logwarn('Behavior version ID is the same.') # Logger.logwarn('Skipping behavior switch, version ID is the same.') # return False # ok, can switch return True def _cleanup_behavior(self, behavior_checksum): del(sys.modules["tmp_%d" % behavior_checksum]) file_path = os.path.join(self._tmp_folder, 'tmp_%d.pyc' % behavior_checksum) os.remove(file_path) def _set_typed_attribute(self, obj, name, value, behavior=''): attr = getattr(obj, name) if type(attr) is int: value = int(value) elif type(attr) is long: value = long(value) elif type(attr) is float: value = float(value) elif type(attr) is bool: value = (value != "0") setattr(obj, name, value) suffix = ' (' + behavior + ')' if behavior != '' else '' rospy.loginfo(name + ' = ' + str(value) + suffix) def _build_contains(self, obj, path): contain_list = dict((path+"/"+key,value) for (key,value) in getattr(obj, 'contains', {}).items()) add_to_list = {} for b_id, b_inst in contain_list.items(): add_to_list.update(self._build_contains(b_inst, b_id)) contain_list.update(add_to_list) return contain_list def _execute_heartbeat(self): thread = threading.Thread(target=self._heartbeat_worker) thread.daemon = True thread.start() def _heartbeat_worker(self): while True: self._pub.publish('/flexbe/heartbeat', Empty()) time.sleep(1) # sec
class ExeRotateState(EventState): def __init__(self, cmd_topic, Kp, max_vel, thr_rad): super(ExeRotateState, self).__init__(output_keys = ['outcome'], input_keys = ['path'], outcomes = ['done']) # Controller gain self._Kp = Kp self._max_vel = max_vel # Controller threshold self._thr = thr_rad # Velocity command interface self._cmd_topic = cmd_topic self._pub = ProxyPublisher({self._cmd_topic: Twist}) # pass required clients as dict (topic: type) # Current state feedback interface self._tf_listener = tf.TransformListener() self._error = False def execute(self, userdata): if self._error: Logger.loginfo("Reading current orientation is failed!") return 'done' else: # check goal is achieved if abs(self.goal_pose - self.cur_pose) < self._thr : return 'done' # control cmd = Twist() cmd.linear.x = 0 cmd.angular.z = self._Kp*(self.goal_pose - self.cur_pose) if cmd.angular.z > self._max_vel : cmd.angular.z = self._max_vel elif cmd.angular.z < -self._max_vel : cmd.angular.z = -self._max_vel self._pub.publish(self._cmd_topic, cmd) try: # get pose feed back [cur_trans, cur_quat] = self._tf_listener.lookupTransform('/map','/base_footprint',rospy.Time(0)) cur_euler = tf.transformations.euler_from_quaternion( cur_quat ) self.cur_pose = cur_euler[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): self._error = True def on_enter(self, userdata): # Set goal orientation as last element of given path goal_euler = self.QuaternionToEuler( userdata.path.poses[0].pose.orientation ) self.goal_pose = goal_euler[2] # Check current pose can be read try: [cur_trans, cur_quat] = self._tf_listener.lookupTransform('/map','/base_footprint',rospy.Time(0)) cur_euler = tf.transformations.euler_from_quaternion( cur_quat ) self.cur_pose = cur_euler[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): self._error = True def on_exit(self, userdata): pass def QuaternionToEuler(self, quat): return tf.transformations.euler_from_quaternion( [quat.x, quat.y, quat.z, quat.w] )
class MoveAlongPath(EventState): """ Lets the robot move along a given path. ># path PoseStamped[] Array of Positions of the robot. ># speed float Speed of the robot <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. """ def __init__(self): """ Constructor """ super(MoveAlongPath, self).__init__(outcomes=["reached", "failed"], input_keys=["path", "speed"]) self._failed = False self._reached = False self._pathTopic = "/controller/path" self._marker_topic = "/debug/path" self._pub = ProxyPublisher({self._pathTopic: MoveBaseActionPath, self._marker_topic: MarkerArray}) def execute(self, userdata): """ Execute this state """ if self._failed: return "failed" if self._reached: return "reached" def on_enter(self, userdata): ma = MarkerArray() self._path = MoveBaseActionPath() for i in range(len(userdata.path.poses)): marker = Marker(type=Marker.ARROW) marker.header = userdata.path.header marker.pose = userdata.path.poses[i].pose marker.scale.x = 0.2 marker.scale.y = 0.02 marker.scale.z = 0.02 marker.color.b = 1.0 marker.color.r = 0.9 - 0.7 * i / len(userdata.path.poses) marker.color.g = 0.9 - 0.7 * i / len(userdata.path.poses) marker.color.a = 0.8 - 0.5 * i / len(userdata.path.poses) marker.id = i ma.markers.append(marker) self._failed = False self._path.goal.target_path.poses = userdata.path.poses self._path.goal.target_path.header.frame_id = "map" self._pub.publish(self._pathTopic, self._path) self._pub.publish(self._marker_topic, ma) self._reached = True def on_stop(self): pass def on_exit(self, userdata): pass def on_pause(self): pass def on_resume(self, userdata): self.on_enter(userdata)
class EventState(OperatableState): """ A state that allows implementing certain events. """ def __init__(self, *args, **kwargs): super(EventState, self).__init__(*args, **kwargs) self._entering = True self._skipped = False self.__execute = self.execute self.execute = self._event_execute self._paused = False self._last_active_container = None self._feedback_topic = 'flexbe/command_feedback' self._repeat_topic = 'flexbe/command/repeat' self._pause_topic = 'flexbe/command/pause' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _event_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._pause_topic): msg = self._sub.get_last_msg(self._pause_topic) if msg.data: self._sub.remove_last_msg(self._pause_topic) rospy.loginfo("--> Pausing in state %s", self.name) self._pub.publish(self._feedback_topic, CommandFeedback(command="pause")) self._last_active_container = PriorityContainer.active_container # claim priority to propagate pause event PriorityContainer.active_container = self._get_path() self._paused = True if self._paused and not PreemptableState.preempt: self._notify_skipped() return self._loopback_name if self._entering: self._entering = False self.on_enter(*args, **kwargs) if self._skipped and not PreemptableState.preempt: self._skipped = False self.on_resume(*args, **kwargs) execute_outcome = self.__execute(*args, **kwargs) repeat = False if self._is_controlled and self._sub.has_msg(self._repeat_topic): rospy.loginfo("--> Repeating state %s", self.name) self._sub.remove_last_msg(self._repeat_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="repeat")) repeat = True if execute_outcome != self._loopback_name and not PreemptableState.switching and not PreemptableState.preempt \ or repeat: self._entering = True self.on_exit(*args, **kwargs) return execute_outcome def _notify_skipped(self): if not self._skipped: self.on_pause() self._skipped = True if self._is_controlled and self._sub.has_msg(self._pause_topic): msg = self._sub.get_last_msg(self._pause_topic) if not msg.data: self._sub.remove_last_msg(self._pause_topic) rospy.loginfo("--> Resuming in state %s", self.name) self._pub.publish(self._feedback_topic, CommandFeedback(command="resume")) PriorityContainer.active_container = self._last_active_container self._last_active_container = None self._paused = False super(EventState, self)._notify_skipped() def _enable_ros_control(self): super(EventState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._repeat_topic, Empty) self._sub.subscribe(self._pause_topic, Bool) def _disable_ros_control(self): super(EventState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._repeat_topic) self._sub.unsubscribe_topic(self._pause_topic) self._last_active_container = None if self._paused: PriorityContainer.active_container = None # Events # (just implement the ones you need) def on_start(self): """ Will be executed once when the behavior starts. """ pass def on_stop(self): """ Will be executed once when the behavior stops or is preempted. """ pass def on_pause(self): """ Will be executed each time this state is paused. """ pass def on_resume(self, userdata): """ Will be executed each time this state is resumed. """ pass def on_enter(self, userdata): """ Will be executed each time the state is entered from any other state (but not from itself). """ pass def on_exit(self, userdata): """ Will be executed each time the state will be left to any other state (but not to itself). """ pass
class PrayingMantisCalibrationSM(Behavior): ''' A behavior that moves ATLAS into the "praying mantis" pose upon startup in order to get consistent joint encoder offsets for calibration purposes. ''' def __init__(self): super(PrayingMantisCalibrationSM, self).__init__() self.name = 'Praying Mantis Calibration' # parameters of this behavior # references to used behaviors # Additional initialization code can be added inside the following tags # [MANUAL_INIT] self._offset_topic = "/flor/controller/encoder_offsets" self._pub = ProxyPublisher({self._offset_topic: JointTrajectory}) self._joint_limits = AtlasDefinitions.arm_joint_limits # Define 90 percent positions for both arms (order of joints same as in _joint_names attribute) # atlas_v5 # - account for fall protection pads # - ignore the lower 3 joints, ie, the electric motor ones left_calib_upper = [-1.4252, -1.4649, +0.1588, +2.2767, +0.1, +0.1, +0.1] left_calib_lower = [+0.5470, +1.2355, +2.9297, +0.1191, -0.1, +1.0, -0.1] right_calib_upper = [+1.4914, +1.4296, +0.2118, -2.2899, +0.1, +0.1, +0.1] right_calib_lower = [-0.5470, -1.2355, +2.9297, -0.1191, -0.1, -1.0, -0.1] # # atlas_v5 (without shoulder pads) # left_calib_upper = [+0.5470, +1.2355, +2.9297, +2.1576, +0.1, +0.1, +0.1] # left_calib_lower = [-1.1869, -1.4296, +0.2118, +0.1191, -1.3, +1.0, -0.1] # right_calib_upper = [-0.5470, -1.2355, +2.9297, -2.1576, +0.1, +0.1, +0.1] # right_calib_lower = [+1.1869, +1.4296, +0.2118, -0.1191, -1.3, -1.0, -0.1] self._joint_calib = {'left_arm': {'upper': left_calib_upper, 'lower': left_calib_lower}, 'right_arm': {'upper': right_calib_upper, 'lower': right_calib_lower} } self._joint_names = AtlasDefinitions.arm_joint_names # [/MANUAL_INIT] # Behavior comments: # O 47 211 /Perform_Checks/Manipulate_Limits # Without this output_key, Check Behavior complains. Because traj_past_limits could in theory be undefined during runtime. def create(self): initial_mode = "stand" motion_mode = "manipulate" mantis_mode = "manipulate_limits" percent_past_limits = 0.10 # before: 0.075 # x:788 y:72, x:474 y:133 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.target_limits = 'upper' _state_machine.userdata.cycle_counter = 1 _state_machine.userdata.stand_posture = None # calculated _state_machine.userdata.offsets = {'left_arm': dict(), 'right_arm': dict()} # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._percent_past_limits = percent_past_limits # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory() # [/MANUAL_CREATE] # x:222 y:281, x:349 y:167 _sm_determine_offsets_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets']) with _sm_determine_offsets_0: # x:61 y:53 OperatableStateMachine.add('Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={'retrieved': 'Determine_Closest_Limits_Left', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'joint_positions': 'joint_positions'}) # x:319 y:54 OperatableStateMachine.add('Determine_Closest_Limits_Left', CalculationState(calculation=self.get_closest_limits_left), transitions={'done': 'Store_Offsets_Left'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'joint_positions', 'output_value': 'joint_limits'}) # x:598 y:162 OperatableStateMachine.add('Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={'retrieved': 'Determine_Closest_Limits_Right', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'joint_positions': 'joint_positions'}) # x:584 y:275 OperatableStateMachine.add('Determine_Closest_Limits_Right', CalculationState(calculation=self.get_closest_limits_right), transitions={'done': 'Store_Offsets_Right'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'joint_positions', 'output_value': 'joint_limits'}) # x:608 y:54 OperatableStateMachine.add('Store_Offsets_Left', FlexibleCalculationState(calculation=self.store_offsets_left, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'Get_Right_Joint_Positions'}, autonomy={'done': Autonomy.Off}, remapping={'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets'}) # x:340 y:274 OperatableStateMachine.add('Store_Offsets_Right', FlexibleCalculationState(calculation=self.store_offsets_right, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets'}) # x:528 y:401, x:707 y:282 _sm_manipulate_limits_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets', 'traj_past_limits']) with _sm_manipulate_limits_1: # x:100 y:156 OperatableStateMachine.add('Prevent_Runtime_Failure', CalculationState(calculation=lambda x: dict()), transitions={'done': 'Go_to_MANIPULATE_LIMITS'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'cycle_counter', 'output_value': 'traj_past_limits'}) # x:387 y:55 OperatableStateMachine.add('Wait_for_Control_Mode_change', WaitState(wait_time=1.0), transitions={'done': 'Get_Left_Joint_Positions'}, autonomy={'done': Autonomy.Low}) # x:895 y:279 OperatableStateMachine.add('Gen_Traj_from_90%_to_110%', CalculationState(calculation=self.gen_traj_past_limits), transitions={'done': 'Go_to_110%_Joint_Limits'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'current_joint_values', 'output_value': 'traj_past_limits'}) # x:893 y:391 OperatableStateMachine.add('Go_to_110%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=['left_arm_traj_controller', 'right_arm_traj_controller']), transitions={'done': 'Determine_Offsets', 'failed': 'Determine_Offsets'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'trajectories': 'traj_past_limits'}) # x:651 y:385 OperatableStateMachine.add('Determine_Offsets', _sm_determine_offsets_0, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cycle_counter': 'cycle_counter', 'offsets': 'offsets'}) # x:648 y:54 OperatableStateMachine.add('Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={'retrieved': 'Get_Right_Joint_Positions', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_positions': 'joint_positions_left'}) # x:904 y:53 OperatableStateMachine.add('Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={'retrieved': 'Generate_Joint_Positions_Struct', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_positions': 'joint_positions_right'}) # x:886 y:168 OperatableStateMachine.add('Generate_Joint_Positions_Struct', FlexibleCalculationState(calculation=lambda ik: {'left_arm': ik[0], 'right_arm': ik[1]}, input_keys=['left', 'right']), transitions={'done': 'Gen_Traj_from_90%_to_110%'}, autonomy={'done': Autonomy.Off}, remapping={'left': 'joint_positions_left', 'right': 'joint_positions_right', 'output_value': 'current_joint_values'}) # x:92 y:55 OperatableStateMachine.add('Go_to_MANIPULATE_LIMITS', ChangeControlModeActionState(target_mode=mantis_mode), transitions={'changed': 'Wait_for_Control_Mode_change', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) # x:574 y:247, x:276 y:549 _sm_update_calibration_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['offsets']) with _sm_update_calibration_2: # x:46 y:44 OperatableStateMachine.add('Process_Offsets', CalculationState(calculation=self.process_offsets), transitions={'done': 'Print_Offset_Info'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'offsets'}) # x:227 y:45 OperatableStateMachine.add('Print_Offset_Info', CalculationState(calculation=self.print_offset_info), transitions={'done': 'Publish_Offsets'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'none'}) # x:390 y:158 OperatableStateMachine.add('Ask_Perform_Update', OperatorDecisionState(outcomes=['update', 'no_update'], hint="Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={'update': 'Convert_Offset_Data', 'no_update': 'finished'}, autonomy={'update': Autonomy.Full, 'no_update': Autonomy.Full}) # x:232 y:337 OperatableStateMachine.add('Update_Calibration', UpdateJointCalibrationState(joint_names=self._joint_names['left_arm'][0:4] + self._joint_names['right_arm'][0:4]), transitions={'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed'}, autonomy={'updated': Autonomy.Low, 'failed': Autonomy.High}, remapping={'joint_offsets': 'offset_list'}) # x:241 y:242 OperatableStateMachine.add('Convert_Offset_Data', CalculationState(calculation=lambda o: o['left_arm']['avg'] + o['right_arm']['avg']), transitions={'done': 'Update_Calibration'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'offset_list'}) # x:522 y:337 OperatableStateMachine.add('Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:246 y:445 OperatableStateMachine.add('Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:399 y:44 OperatableStateMachine.add('Publish_Offsets', CalculationState(calculation=self.publish_offsets), transitions={'done': 'Ask_Perform_Update'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'offsets', 'output_value': 'none'}) # x:978 y:197, x:394 y:80 _sm_perform_checks_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'target_limits', 'offsets'], output_keys=['cycle_counter', 'offsets']) with _sm_perform_checks_3: # x:105 y:74 OperatableStateMachine.add('Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={'changed': 'Gen_Traj_to_90%_Limits', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) # x:653 y:274 OperatableStateMachine.add('Manipulate_Limits', _sm_manipulate_limits_1, transitions={'finished': 'Gen_Traj_back_to_90%_Limits', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cycle_counter': 'cycle_counter', 'offsets': 'offsets', 'traj_past_limits': 'traj_past_limits'}) # x:903 y:78 OperatableStateMachine.add('Increment_Cycle_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'cycle_counter', 'output_value': 'cycle_counter'}) # x:344 y:277 OperatableStateMachine.add('Move_to_90%_Joint_Limits', MoveitStartingPointState(vel_scaling=0.3), transitions={'reached': 'Manipulate_Limits', 'failed': 'Move_to_90%_Joint_Limits'}, autonomy={'reached': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'trajectories': 'trajectories_90'}) # x:114 y:276 OperatableStateMachine.add('Gen_Traj_to_90%_Limits', CalculationState(calculation=self.gen_traj_pre_limits), transitions={'done': 'Move_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'target_limits', 'output_value': 'trajectories_90'}) # x:636 y:78 OperatableStateMachine.add('Go_back_to_90%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=['left_arm_traj_controller', 'right_arm_traj_controller']), transitions={'done': 'Increment_Cycle_counter', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'trajectories': 'traj_back_to_90'}) # x:636 y:172 OperatableStateMachine.add('Gen_Traj_back_to_90%_Limits', FlexibleCalculationState(calculation=self.gen_traj_back_from_limits, input_keys=['trajectories_90', 'traj_past_limits']), transitions={'done': 'Go_back_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={'trajectories_90': 'trajectories_90', 'traj_past_limits': 'traj_past_limits', 'output_value': 'traj_back_to_90'}) with _state_machine: # x:110 y:52 OperatableStateMachine.add('Initial_Control_Mode', ChangeControlModeActionState(target_mode=initial_mode), transitions={'changed': 'Perform_Checks', 'failed': 'failed'}, autonomy={'changed': Autonomy.High, 'failed': Autonomy.High}) # x:712 y:317 OperatableStateMachine.add('Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={'changed': 'Update_Calibration', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) # x:122 y:302 OperatableStateMachine.add('Perform_Checks', _sm_perform_checks_3, transitions={'finished': 'Are_We_Done_Yet?', 'failed': 'Intermediate_Mode_before_exit'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cycle_counter': 'cycle_counter', 'target_limits': 'target_limits', 'offsets': 'offsets'}) # x:126 y:505 OperatableStateMachine.add('Are_We_Done_Yet?', DecisionState(outcomes=["done", "more"], conditions=lambda counter: "done" if counter >= 2 else "more"), transitions={'done': 'Intermediate_Mode_before_exit', 'more': 'Setup_next_Cycle'}, autonomy={'done': Autonomy.Low, 'more': Autonomy.High}, remapping={'input_value': 'cycle_counter'}) # x:15 y:404 OperatableStateMachine.add('Setup_next_Cycle', CalculationState(calculation=lambda lim: 'lower' if lim == 'upper' else 'upper'), transitions={'done': 'Perform_Checks'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'target_limits', 'output_value': 'target_limits'}) # x:725 y:186 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_2, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'offsets': 'offsets'}) # x:726 y:427 OperatableStateMachine.add('Move_to_Stand_Posture', MoveitStartingPointState(vel_scaling=0.3), transitions={'reached': 'Initial_Mode_before_exit', 'failed': 'Move_to_Stand_Posture'}, autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'trajectories': 'stand_posture'}) # x:412 y:427 OperatableStateMachine.add('Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={'changed': 'Move_to_Stand_Posture', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) return _state_machine # Private functions can be added inside the following tags # [MANUAL_FUNC] def gen_traj_pre_limits(self, limits_side): """Create trajectories for going to 90 percent of joint limits (either upper or lower limits)""" joint_config = {'left_arm': self._joint_calib['left_arm'][limits_side], 'right_arm': self._joint_calib['right_arm'][limits_side] } return AtlasFunctions.gen_arm_trajectory_from_joint_configuration(joint_config) def _get_closest_limits(self, side, current_values): """ Selects the closest limit with respect to the current value (upper or lower bound). """ limits = self._joint_limits[side] closest_limit = list() for i in range(len(current_values)): near_limit = 'upper' if abs(limits['upper'][i] - current_values[i]) < abs(limits['lower'][i] - current_values[i]) else 'lower' closest_limit.append(limits[near_limit][i]) rospy.loginfo("Limit joint positions: %s" % str(closest_limit)) rospy.loginfo("Current joint positions: %s" % str(current_values)) return closest_limit def get_closest_limits_left(self, current_values): return self._get_closest_limits('left_arm', current_values) def get_closest_limits_right(self, current_values): return self._get_closest_limits('right_arm', current_values) def gen_traj_past_limits(self, current_joint_values): """ Given all joint limits, generate a trajectory that takes the joints to 110%% percent past limits. atlas_v5 update: Do not push the lower 3 joints (electric ones) path the limits. """ result = dict() for arm in ['left_arm', 'right_arm']: current_values = current_joint_values[arm] arm_limits = self._get_closest_limits(arm, current_values) arm_target = list() arm_effort = list() percentage = self._percent_past_limits # Push the upper 4 joints against the limits for i in range(0,4): near_limit = 'upper' if self._joint_limits[arm]['upper'][i] == arm_limits[i] else 'lower' limit_range = self._joint_limits[arm]['upper'][i] - self._joint_limits[arm]['lower'][i] offset_sign = 1 if near_limit is 'upper' else -1 arm_target.append(arm_limits[i] + offset_sign * percentage * limit_range) arm_effort.append(float(offset_sign)) # "Ignore" the lower 3 joints (electric motor ones) for i in range(4,7): arm_target.append(current_values[i]) arm_effort.append(0.0) # Zero effort stands for not applying additional force trajectory = JointTrajectory() trajectory.joint_names = self._joint_names[arm] point = JointTrajectoryPoint() point.positions = arm_target point.velocities = [0.0] * len(arm_target) # David's controller expects zero velocities point.effort = arm_effort point.time_from_start = rospy.Duration.from_sec(2.5) trajectory.points.append(point) # rospy.loginfo("110%% joint positions for %s arm: %s" % (arm, str(arm_target[0:4]))) # Only report the relevant joints result[arm] = trajectory return result def gen_traj_back_from_limits(self, input_keys): """The resulting trajectory points are the same as for going to 90%% of limits, but with the efforts set for David's controllers.""" traj_pre_limits = input_keys[0] traj_past_limits = input_keys[1] traj_back_to_90 = dict() for arm in ['left_arm', 'right_arm']: trajectory = traj_pre_limits[arm] # Start with 90% of joint limits as the trajectory points trajectory.points[0].effort = traj_past_limits[arm].points[0].effort # Set efforts as per David's controllers trajectory.points[0].time_from_start = rospy.Duration.from_sec(1.0) # David's controller expects zero velocities trajectory.points[0].velocities = [0.0] * len(trajectory.points[0].positions) traj_back_to_90[arm] = trajectory return traj_back_to_90 def store_offsets(self, side, input_keys): limits = input_keys[0][0:4] # Ignore the lower 3 joints values = input_keys[1][0:4] # --//-- --//-- offsets = input_keys[2] counter = input_keys[3] offsets[side][counter] = [limit - value for limit, value in zip(limits, values)] msg = JointTrajectory() msg.joint_names = self._joint_names[side][0:4] # Ignore the lower 3 joints point = JointTrajectoryPoint() point.positions = values point.velocities = offsets[side][counter] msg.points.append(point) self._pub.publish(self._offset_topic, msg) Logger.loginfo("Publishing %s arm offsets to %s" % (side, self._offset_topic)) return offsets def publish_offsets(self, offsets, arms = ['left_arm', 'right_arm'], current_values = []): for side in arms: msg = JointTrajectory() msg.joint_names = self._joint_names[side] point = JointTrajectoryPoint() point.positions = current_values point.velocities = offsets[side]['avg'] msg.points.append(point) self._pub.publish(self._offset_topic, msg) Logger.loginfo("Publishing %s arm offsets to %s" % (side, self._offset_topic)) def store_offsets_left(self, input_keys): return self.store_offsets('left_arm', input_keys) def store_offsets_right(self, input_keys): return self.store_offsets('right_arm', input_keys) def process_offsets(self, offsets): for side in ['left_arm', 'right_arm']: # transposes list of lists from iteration,joint to joint,iteration iteration_values = map(list, zip(*offsets[side].values())) # Calculate the average offset and the deviation from the average offsets[side]['avg'] = [sum(joint_entries)/float(len(joint_entries)) for joint_entries in iteration_values] offsets[side]['diff'] = [max(map(lambda x: abs(x-avg),joint_entries)) for joint_entries,avg in zip(iteration_values, offsets[side]['avg'])] return offsets def print_offset_info(self, offsets): sides = ['left_arm', 'right_arm'] for side in sides: Logger.loginfo("Joint order (%s): %s" % (side, str(self._joint_names[side][0:4]))) rounded_offsets = [round(offset, 3) for offset in offsets[side]['avg']] # round due to comms_bridge Logger.loginfo("Offsets (%s): %s" % (side, str(rounded_offsets))) # Logger.loginfo("Max deviation from average (%s): %s" % (side, str(offsets[side]['diff']))) pprint.pprint(offsets) # Pretty print to the "onboard" terminal
class JoystickJointControl(EventState): ''' Control joint position with joystick. If button is pressed joint is moving with defined in configuration speed. For axes speed is modulated with axis value. If speed is equal to zero, then position of joint is defined directly by axis position. Note that one button may control multiple joints. -- joints_topic string Topic with current robot pose (JointState) -- goal_joints_topic string Topic to publish desired joint positions. -- joy_topic string Joystick -- buttons object Array with tuples: (button number, speed, joint name, min, max). -- axes object Array with tuples: (axis number, speed, joint name, min, max). -- timeout number Exit state if no joystick input is provided for timeout. If timeout is negative waits forever. <= timeout No joystick activity for given timeout. ''' def __init__(self, joints_topic='joint_states', goal_joints_topic='goal_joint_states', joy_topic = 'joy', buttons = [], axes = [], timeout = 5.0): super(JoystickJointControl, self).__init__(outcomes = ['done']) # store state parameter for later use. self._joints_topic = joints_topic self._goal_topic = goal_joints_topic self._joy_topic = joy_topic self._timeout = timeout # Event (button or axis) description type. # EventDescription = namedtuple('EventDescription', ['ev_num', 'speed', 'joint', 'min', 'max']) class EventDescription(): def __init__(self, args): # check arg type if not isinstance(args, (list,tuple)) or len(args) != 5: raise ValueError('Button/axis event description must be a tuple with 5 elements.') # parse arg in fields ( self.ev_num, self.speed, self.joint, self.min, self.max ) = args # check fields types if not isinstance(self.ev_num, int): raise ValueError('Button/axis number must be integer.') if not isinstance(self.speed, (int, float)): raise ValueError('Speself must be float.') if not isinstance(self.max, (int, float)) or not isinstance(self.min, (int,float)): raise ValueError('Min/max position must be float.') if not isinstance(self.joint, str): raise ValueError('Joint name must be string.') # parse buttons and axes self._buttons = [ EventDescription(button_conf) for button_conf in buttons ] self._axes = [ EventDescription(axis_conf) for axis_conf in axes ] # subscribe to topics self._joy_subscriber = ProxySubscriberCached({ self._joy_topic: Joy }) self._joints_subscriber = ProxySubscriberCached({ self._joints_topic: JointState }) # setup publiser proxies self._goal_publisher = ProxyPublisher({ self._goal_topic: JointState }) # buffers self._timestamp = None self._activity_timestamp = None self._goal_position = {} # map { joint: position } for all controlled joints def on_enter(self, userdata): # reset timestamp self._timestamp = rospy.Time.now() self._activity_timestamp = self._timestamp # fill joint goal array self._goal_position = {} # get current joints positions joints_msg = self._joints_subscriber.get_last_msg(self._joints_topic) # check message if joints_msg == None or len(joints_msg.name) != len (joints_msg.position): Logger.logerr('JoystickJointControl: robot pose is unavailable or incorrect on topic `%s`' % self._joints_topic) raise RuntimeError('JoystickJointControl: robot pose is unavailable or incorrect on topic `%s`' % self._joints_topic) for event in self._buttons + self._axes: # find coresponding joint is_found = False for i in range(0,len(joints_msg.name)): if joints_msg.name[i] == event.joint: is_found = True self._goal_position[event.joint] = clip(joints_msg.position[i], event.min, event.max) # check if joint is found if not is_found: Logger.logerr('JoystickJointControl: unable to determine `%s` joint position. (min + max)/2 is used.' % event.joint) self._goal_position[event.joint] = (event.min + event.max) / 2.0 # default value Logger.loginfo('JoystickJointControl: start monitoring.') def execute(self, userdata): # calculate time shift timestamp = rospy.Time.now() dt = (timestamp - self._timestamp).to_sec() self._timestamp = timestamp activity_detected = False # check if new message is available if self._joy_subscriber.has_msg(self._joy_topic): # get messages joy_msg = self._joy_subscriber.get_last_msg(self._joy_topic) # TODO skip non-used axes and buttons # check if any button is pressed for i in range(0, len(joy_msg.buttons)): # skip not pressed buttons if not joy_msg.buttons[i]: continue # find event handlers for event in self._buttons: if event.ev_num == i: # modify position accordingly self._goal_position[event.joint] = clip(self._goal_position[event.joint] + event.speed*dt, event.min, event.max) activity_detected = True # check if any axis is non-zero for i in range(0, len(joy_msg.axes)): # find event handlers for event in self._axes: if event.ev_num == i: # modify position accordingly if event.speed != 0.0: # add shift self._goal_position[event.joint] = clip(self._goal_position[event.joint] + event.speed*joy_msg.axes[i]*dt, event.min, event.max) else: # set position self._goal_position[event.joint] = 0.5*((event.max - event.min)*joy_msg.axes[i] + event.max + event.min) if joy_msg.axes[i] != 0.0: activity_detected = True # publish pose goal_msg = JointState() goal_msg.header.stamp = timestamp for joint, position in self._goal_position.items(): goal_msg.name.append(joint) goal_msg.position.append(position) self._goal_publisher.publish(self._goal_topic, goal_msg) # check activity if activity_detected: self._activity_timestamp = timestamp return None # check activity timeout if (timestamp - self._activity_timestamp).to_sec() > self._timeout: return 'done' else: return None def on_exit(self, userdata): Logger.loginfo('JoystickJointControl: stop monitoring.')
class OperatableStateMachine(PreemptableStateMachine): """ A state machine that can be operated. It synchronizes its current state with the mirror and supports some control mechanisms. """ autonomy_level = 3 silent_mode = False def __init__(self, *args, **kwargs): super(OperatableStateMachine, self).__init__(*args, **kwargs) self._message = None self.id = None self.autonomy = None self._autonomy = {} self._ordered_states = [] self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() @staticmethod def add(label, state, transitions = None, autonomy = None, remapping = None): """ Add a state to the opened state machine. @type label: string @param label: The label of the state being added. @param state: An instance of a class implementing the L{State} interface. @param transitions: A dictionary mapping state outcomes to other state labels or container outcomes. @param autonomy: A dictionary mapping state outcomes to their required autonomy level @param remapping: A dictionary mapping local userdata keys to userdata keys in the container. """ self = StateMachine._currently_opened_container() # add loopback transition to loopback states if isinstance(state, LoopbackState): transitions[LoopbackState._loopback_name] = label autonomy[LoopbackState._loopback_name] = -1 self._ordered_states.append(state) state.name = label state.transitions = transitions state.autonomy = autonomy state._parent = self StateMachine.add(label, state, transitions, remapping) self._autonomy[label] = autonomy def replace(self, new_state): old_state = self._states[new_state.name] new_state.transitions = old_state.transitions new_state.autonomy = old_state.autonomy new_state._parent = old_state._parent self._ordered_states[self._ordered_states.index(old_state)] = new_state self._states[new_state.name] = new_state def destroy(self): self._notify_stop() self._disable_ros_control() self._sub.unsubscribe_topic('/flexbe/command/autonomy') self._sub.unsubscribe_topic('/flexbe/command/sync') self._sub.unsubscribe_topic('/flexbe/request_mirror_structure') StateLogger.shutdown() def confirm(self, name, id): """ Confirms the state machine and triggers the creation of the structural message. It is mandatory to call this function at the top-level state machine between building it and starting its execution. @type name: string @param name: The name of this state machine to identify it. """ self.name = name self.id = id self._pub.createPublisher('/flexbe/mirror/sync', BehaviorSync, _latch = True) # Update mirror with currently active state (high bandwidth mode) self._pub.createPublisher('/flexbe/mirror/preempt', Empty, _latch = True) # Preempts the mirror self._pub.createPublisher('/flexbe/mirror/structure', ContainerStructure) # Sends the current structure to the mirror self._pub.createPublisher('/flexbe/log', BehaviorLog) # Topic for logs to the GUI self._pub.createPublisher('/flexbe/command_feedback', CommandFeedback) # Gives feedback about executed commands to the GUI self._sub.subscribe('/flexbe/command/autonomy', UInt8, self._set_autonomy_level) self._sub.subscribe('/flexbe/command/sync', Empty, self._sync_callback) self._sub.subscribe('/flexbe/request_mirror_structure', Int32, self._mirror_structure_callback) StateLogger.initialize(name) if OperatableStateMachine.autonomy_level != 255: self._enable_ros_control() rospy.sleep(0.5) # no clean way to wait for publisher to be ready... self._notify_start() def _set_autonomy_level(self, msg): """ Sets the current autonomy level. """ if OperatableStateMachine.autonomy_level != msg.data: rospy.loginfo('--> Autonomy changed to %d', msg.data) if msg.data < 0: self.preempt() else: OperatableStateMachine.autonomy_level = msg.data self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="autonomy", args=[])) def _sync_callback(self, msg): rospy.loginfo("--> Synchronization requested...") msg = BehaviorSync() msg.behavior_id = self.id msg.current_state_checksum = zlib.adler32(self._get_deep_state()._get_path()) self._pub.publish('/flexbe/mirror/sync', msg) self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="sync", args=[])) rospy.loginfo("<-- Sent synchronization message for mirror.") def _mirror_structure_callback(self, msg): rospy.loginfo("--> Creating behavior structure for mirror...") msg = self._build_msg('') msg.behavior_id = self.id self._pub.publish('/flexbe/mirror/structure', msg) rospy.loginfo("<-- Sent behavior structure for mirror.") def _transition_allowed(self, label, outcome): return self._autonomy[label][outcome] < OperatableStateMachine.autonomy_level def _build_msg(self, prefix, msg = None): """ Adds this state machine to the initial structure message. @type prefix: string @param prefix: A path consisting of the container hierarchy containing this state. @type msg: ContainerStructure @param msg: The message that will finally contain the structure message. """ # set children children = [] for state in self._ordered_states: children.append(str(state.name)) # set name name = prefix + self.name if msg is None: # top-level state machine (has no transitions) self._message = ContainerStructure() outcomes = list(self._outcomes) transitions = None autonomy = None else: # lower-level state machine self._message = msg outcomes = list(self.transitions) # set transitions and autonomy transitions = [] autonomy = [] for i in range(len(self.transitions)): outcome = outcomes[i] if outcome == 'preempted': # set preempt transition transitions.append('preempted') autonomy.append(-1) else: transitions.append(str(self.transitions[outcome])) autonomy.append(self.autonomy[outcome]) # add to message self._message.containers.append(Container(name, children, outcomes, transitions, autonomy)) # build message for children for state in self._ordered_states: state._build_msg(name+'/', self._message) # top-level state machine returns the message if msg is None: return self._message def _notify_start(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_start() if isinstance(state, OperatableStateMachine): state._notify_start() def _enable_ros_control(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state._enable_ros_control() if isinstance(state, OperatableStateMachine): state._enable_ros_control() def _notify_stop(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_stop() state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._notify_stop() def _disable_ros_control(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._disable_ros_control()
class CreateTimedStopState(EventState): ''' This state publishes a constant zero TwistStamped command based on parameters. The state monitors the iRobot Create odometry and returns a failed outcome if speed is not near zero within the timeout -- timeout float Time which needs to have passed since the behavior started. (default: 0.2) -- odom_topic string topic of the iRobot Create sensor state (default: '/create_node/odom') -- cmd_topic string topic name of the robot command (default: '/create_node/cmd_vel') <= done Given time has passed. <= failed A bumper was activated prior to completion ''' def __init__(self, timeout=0.2, cmd_topic='/create_node/cmd_vel', odom_topic='/create_node/odom'): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(CreateTimedStopState, self).__init__(outcomes=['done', 'failed']) # Store state parameter for later use. self._timeout = rospy.Duration(timeout) self._twist = TwistStamped() # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._done = None # Track the outcome so we can detect if transition is blocked self._odom_topic = odom_topic self._cmd_topic = cmd_topic self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry}) self._pub = ProxyPublisher({self._cmd_topic: TwistStamped}) def execute(self, userdata): # This method is called periodically while the state is active. # If no outcome is returned, the state will stay active. if (rospy.Time.now() - self._start_time) > self._timeout: # Normal completion, verify stoppage if (self._sub.has_msg(self._odom_topic)): odom = self._sub.get_last_msg(self._odom_topic) speed = odom.twist.twist.linear.x * odom.twist.twist.linear.x + odom.twist.twist.angular.z * odom.twist.twist.angular.z if (speed > 5.0e-4): Logger.logwarn( 'Stop failed twist linear = %f,%f,%f angular=%f, %f, %f' % (odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.linear.z, odom.twist.twist.angular.x, odom.twist.twist.angular.y, odom.twist.twist.angular.z)) self._done = 'failed' return 'failed' self._done = 'done' return 'done' # Normal operation publish the zero twist self._twist.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, self._twist) return None def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._start_time = rospy.Time.now() self._done = None # reset the completion flag
class KylePubInputState(EventState): ''' This state publishes an open loop constant TwistStamped command based on parameters. -- target_time float Time which needs to have passed since the behavior started. -- velocity float Body velocity (m/s) -- rotation_rate float Angular rotation (radians/s) -- cmd_topic string topic name of the robot velocity command (default: 'cmd_vel') <= done Given time has passed. ''' def __init__(self, cmd_topic='cmd_vel', increaseBy = 1): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(KylePubInputState, self).__init__(outcomes = ['done'], input_keys=['valueToIncrease']) # Store state parameter for later use. #self._target_time = rospy.Duration(target_time #self._twist.twist.linear.x = velocity #self._twist.twist.angular.z = rotation_rate # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._count = Int32() self._increaseBy = Int32() self._increaseBy.data = increaseBy self._done = None # Track the outcome so we can detect if transition is blocked self._cmd_topic = cmd_topic self._pub = ProxyPublisher( {self._cmd_topic: Int32}) def execute(self, userdata): # This method is called periodically while the state is active. # If no outcome is returned, the state will stay active. # if (self._done): # We have completed the state, and therefore must be blocked by autonomy level # Stop the robot, but and return the prior outcome # ts = TwistStamped() # Zero twist to stop if blocked # ts.header.stamp = rospy.Time.now() # self._pub.publish(self._cmd_topic, ts) # return self._done # if rospy.Time.now() - self._start_time > self._target_time: # Normal completion, do not bother repeating the publish # self._done = 'done' # return 'done' self._count.data = userdata.valueToIncrease.data self._count.data = self._count.data self._count.data = self._count.data + self._increaseBy.data # Normal operation #self._twist.twist.linear.x = 1.0 #self._twist.twist.angular.z = 1.0 self._pub.publish(self._cmd_topic, self._count) return 'done' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._start_time = rospy.Time.now() self._done = None # reset the completion flag
class PurePursuitState(EventState): """ This state calculates the twist required to follow a constant curvature arc to the pure pursuit intersection point. The command is published as a TwistStamped command based on parameters. If arc motion is used, the arc should be less than or equal to pi/2 radians. Use multiple targets for longer arcs. -- desired_velocity float Desired velocity in m/s (default: 0.2) -- max_rotation_rate float Maximum rotation rate radians/s (default: 10.0) -- target_frame: string Coordinate frame of target point (default: 'map') -- target_x: float Target point x-coordinate (m) -- target_y: float Target point y-coordinate (m) -- target_type: string Desired motion ('line','arc') (default: 'line') -- lookahead_distance: float Lookahead distance (m) (default: 0.25) -- timeout float Transform timeout (seconds) (default: 0.08) -- recover_mode bool Recover path (typically on startup) (default: False) -- center_x: float Center point x-coordinate for circle defining arc motion (default: 0.0) -- center_y: float Center point y-coordinate for circle defining arc motion (default: 0.0) -- cmd_topic string topic name of the robot command (default: '/create_node/cmd_vel') -- odometry_topic: string topic of the iRobot Create sensor state (default: '/create_node/odom' -- marker_topic: string topic of the RViz marker used for visualization (default: '/pure_pursuit_marker') -- marker_size: float Size of RViz marker used for target (default: 0.05) ># indice int The index ># path Path The path #> indice int The index + 1 #> plan Path The path <= done Reached the end of target relevance <= continue Continue following the path <= failed A bumper was activated prior to completion """ def __init__(self, desired_velocity=0.2, max_rotation_rate=10.0, target_frame='map', target_x=1.0, target_y=0.1, target_type='line', lookahead_distance=0.25, timeout=0.08, recover_mode=False, center_x=0.0, center_y=0.0, cmd_topic='cmd_vel', odometry_topic='odom', marker_topic='pure_pursuit_marker', marker_size=0.05): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(PurePursuitState, self).__init__(outcomes=['done', 'continue', 'failed'], input_keys=['indice', 'plan'], output_keys=['indice', 'plan']) # Store state parameter for later use. self._twist = TwistStamped() self._twist.twist.linear.x = desired_velocity self._twist.twist.angular.z = 0.0 self._desired_velocity = desired_velocity self._max_rotation_rate = max_rotation_rate # Used to clamp the rotation calculations self._current_position = PointStamped() self._current_position.header.stamp = rospy.Time.now() self._current_position.header.frame_id = target_frame self._target = PointStamped() self._target.header.stamp = rospy.Time.now() self._target.header.frame_id = target_frame self._target.point = Point(target_x, target_y, 0.0) self._prior = PointStamped() self._prior.header.stamp = rospy.Time.now() self._prior.header.frame_id = target_frame self._lookahead = lookahead_distance self._recover_mode = recover_mode self._target_type = target_type self._target_frame = target_frame if (self._target_type == 'arc'): self._radius = np.sqrt((center_x - target_x)**2 + (center_y - target_y)**2) Logger.loginfo( 'Using arc type with center=(%d, %d) target=(%d,%d) radius=%d' % (self._center.point.x, self._center.point.y, self._target.point.x, self._target.point.y, self._radius)) self._odom_frame = 'odom' # Update with the first odometry message self._robot_frame = 'base_footprint' self._failed = False self._timeout = rospy.Duration(timeout) # transform timeout # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._return = None # Track the outcome so we can detect if transition is blocked self._odom_topic = odometry_topic self._marker_topic = marker_topic self._cmd_topic = cmd_topic self._listener = ProxyTransformListener() self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry}) self._pub = ProxyPublisher({self._cmd_topic: TwistStamped}) if (self._marker_topic != ""): self._marker_pub = ProxyPublisher({self._marker_topic: Marker}) else: self._marker_pub = None self._marker = Marker() self._marker.header.frame_id = self._target_frame self._marker.header.stamp = rospy.Time.now() self._marker.ns = "pure_pursuit_waypoints" self._marker.id = int(target_x * 1000000) + int(target_y * 1000) self._marker.type = Marker.SPHERE self._marker.action = Marker.ADD self._marker.pose.position.x = target_x self._marker.pose.position.y = target_y self._marker.pose.position.z = 0.0 self._marker.pose.orientation.x = 0.0 self._marker.pose.orientation.y = 0.0 self._marker.pose.orientation.z = 0.0 self._marker.pose.orientation.w = 1.0 self._marker.scale.x = marker_size self._marker.scale.y = marker_size self._marker.scale.z = marker_size self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.0 self._marker.color.g = 0.0 self._marker.color.b = 1.0 self._reference_marker = Marker() self._reference_marker.header.frame_id = self._target_frame self._reference_marker.header.stamp = rospy.Time.now() self._reference_marker.ns = "pure_pursuit_reference" self._reference_marker.id = 1 self._reference_marker.type = Marker.SPHERE self._reference_marker.action = Marker.ADD self._reference_marker.pose.position.x = target_x self._reference_marker.pose.position.y = target_y self._reference_marker.pose.position.z = 0.0 self._reference_marker.pose.orientation.x = 0.0 self._reference_marker.pose.orientation.y = 0.0 self._reference_marker.pose.orientation.z = 0.0 self._reference_marker.pose.orientation.w = 1.0 self._reference_marker.scale.x = marker_size * 0.75 self._reference_marker.scale.y = marker_size * 0.75 self._reference_marker.scale.z = marker_size * 0.75 self._reference_marker.color.a = 0.0 # Add, but make invisible at first self._reference_marker.color.r = 1.0 self._reference_marker.color.g = 0.0 self._reference_marker.color.b = 1.0 self._local_target_marker = Marker() self._local_target_marker.header.frame_id = self._target_frame self._local_target_marker.header.stamp = rospy.Time.now() self._local_target_marker.ns = "pure_pursuit_target" self._local_target_marker.id = 1 self._local_target_marker.type = Marker.SPHERE self._local_target_marker.action = Marker.ADD self._local_target_marker.pose.position.x = target_x self._local_target_marker.pose.position.y = target_y self._local_target_marker.pose.position.z = 0.0 self._local_target_marker.pose.orientation.x = 0.0 self._local_target_marker.pose.orientation.y = 0.0 self._local_target_marker.pose.orientation.z = 0.0 self._local_target_marker.pose.orientation.w = 1.0 self._local_target_marker.scale.x = marker_size self._local_target_marker.scale.y = marker_size self._local_target_marker.scale.z = marker_size self._local_target_marker.color.a = 0.0 # Add, but make invisible at first self._local_target_marker.color.r = 1.0 self._local_target_marker.color.g = 0.0 self._local_target_marker.color.b = 1.0 # Transform point into odometry frame def transformOdom(self, point): try: # Get transform self._listener.listener().waitForTransform(self._odom_frame, point.header.frame_id, point.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._odom_frame, point) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn('Failed to get the transformation\n%s' % str(e)) self._failed = True return None except Exception as e: Logger.logwarn( 'Failed to get the transformation due to unknown error\n %s' % str(e)) self._failed = True return None # Transform point into map frame def transformMap(self, odometry): odom_position = PointStamped() odom_position.header = odometry.header odom_position.point = odometry.pose.pose.position try: # Get transform self._listener.listener().waitForTransform( self._target_frame, odometry.header.frame_id, odometry.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._target_frame, odom_position) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn( 'Failed to get the transformation to target_frame\n%s' % str(e)) self._failed = True return None except Exception as e: Logger.logwarn( 'Failed to get the transformation to target frame due to unknown error\n %s' % str(e)) self._failed = True return None # Transform point into robot body frame def transformBody(self, point): try: # Get transform self._listener.listener().waitForTransform(self._robot_frame, point.header.frame_id, point.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._robot_frame, point) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn('Failed to get the transformation\n%s' % str(e)) self._failed = True return None except: Logger.logwarn( 'Failed to get the transformation due to unknown error\n') self._failed = True return None def execute(self, userdata): # This method is called periodically while the state is active. # If no outcome is returned, the state will stay active. if (self._return): # We have completed the state, and therefore must be blocked by autonomy level # Stop the robot, but and return the prior outcome ts = TwistStamped() ts.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, ts) return self._return # Get the latest odometry data if (self._sub.has_msg(self._odom_topic)): self._last_odom = self._sub.get_last_msg(self._odom_topic) # Update the current pose self._current_position = self.transformMap(self._last_odom) if (self._failed): self._return = 'failed' return 'failed' # Transform the target points into the current odometry frame self._target.header.stamp = self._last_odom.header.stamp local_target = self._target #self.transformOdom(self._target) # If target point is withing lookahead distance then we are done dr = np.sqrt( (local_target.point.x - self._current_position.point.x)**2 + (local_target.point.y - self._current_position.point.y)**2) if (dr < self._lookahead): Logger.loginfo( ' %s Lookahead circle is past target - done! target=(%f, %f, %f) robot=(%f,%f, %f) dr=%f lookahead=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, dr, self._lookahead)) if (userdata.indice == len(userdata.plan.poses) - 1): self._return = 'done' return 'done' else: self._return = 'continue' return 'continue' # Transform the prior target point into the current odometry frame self._prior.header.stamp = self._last_odom.header.stamp local_prior = self._prior #self.transformOdom(self._prior) if (self._failed): self._return = 'failed' return 'failed' # Assume we can go the desired velocity self._twist.twist.linear.x = self._desired_velocity lookahead = None lookahead = self.calculateLineTwist(local_prior, local_target) if (lookahead is None): # Did not get a lookahead point so we either failed, completed segment, or are deliberately holding the prior velocity (to recover from minor perturbations) if (self._return is not None): Logger.logerr("No lookahead!") return self._return # return what was set (either 'failed' or 'done') # Sanity check the rotation rate if (math.fabs(self._twist.twist.angular.z) > self._max_rotation_rate): self._twist.twist.linear.x = self._desired_velocity * self._max_rotation_rate / math.fabs( self._twist.twist.angular.z) # decrease the speed self._twist.twist.angular.z = math.copysign( self._max_rotation_rate, self._twist.twist.angular.z) # Normal operation - publish the latest calculated twist self._twist.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, self._twist) if (self._marker_pub): self._marker_pub.publish(self._marker_topic, self._reference_marker) self._marker_pub.publish(self._marker_topic, self._local_target_marker) return None def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._start_time = rospy.Time.now() self._return = None # reset the completion flag self._failed = False # reset the failed flag if (self._marker_pub): self._marker.action = Marker.MODIFY self._marker.color.a = 1.0 self._marker.color.r = 0.0 self._marker.color.g = 1.0 # Indicate this target is active self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) if (userdata.indice > 0 and userdata.indice < len(userdata.plan.poses)): Logger.loginfo(" Access data for index %d" % (userdata.indice)) self._target.point = Point( userdata.plan.poses[userdata.indice].pose.position.x, userdata.plan.poses[userdata.indice].pose.position.y, 0.0) self._prior.point = Point( userdata.plan.poses[userdata.indice - 1].pose.position.x, userdata.plan.poses[userdata.indice - 1].pose.position.y, 0.0) Logger.loginfo( " Moving toward target %d =%f,%f from prior=%f,%f" % (userdata.indice, self._target.point.x, self._target.point.y, self._prior.point.x, self._prior.point.y)) else: Logger.logerr( " Invalid index %d - cannot access the path points!" % (userdata.indice)) self._target.point = None self._prior.point = None self._failed = True self._return = 'failed' # Increment the index for the next segment userdata.indice += 1 def on_exit(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. elapsed_time = rospy.Time.now() - self._start_time # userdata.target_point = self._target # Set the user data for passing to next node #Logger.logdebug(" Spent %f seconds in this segment target=%f,%f position=%f,%f" % # (elapsed_time.to_sec(),self._target.point.x, self._target.point.y, # self._current_position.point.x, self._current_position.point.y)) if (self._marker_pub): self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.8 # Indicate this target is no longer active self._marker.color.g = 0.0 self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) def on_start(self): self._return = None # Clear completion flag # Wait for odometry message while (not self._odom_sub.has_msg(self._odom_topic)): Logger.logwarn('Waiting for odometry message from the robot ') rospy.sleep(0.05) while (not self._odom_sub.has_msg(self._odom_topic)): Logger.logwarn( 'Waiting for odometry message to become available from the robot ' ) rospy.sleep(0.25) self._last_odom = self._sub.get_last_msg(self._odom_topic) self._odom_frame = self._last_odom.header.frame_id #Logger.loginfo(' odometry frame id <%s>' % (self._odom_frame)) # Update the target transformation self._target.header.stamp = self._last_odom.header.stamp while (self.transformOdom(self._target) is None): Logger.logwarn( 'Waiting for tf transformations to odometry frame to become available from the robot ' ) rospy.sleep(0.25) self._target.header.stamp = rospy.Time.now() while (self.transformMap(self._last_odom) is None): Logger.logwarn( 'Waiting for tf transformations to map frame become available from the robot ' ) rospy.sleep(0.25) self._last_odom = self._sub.get_last_msg(self._odom_topic) self._current_position = self.transformMap(self._last_odom) # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. if (self._marker_pub): self._marker.action = Marker.ADD self._marker.color.a = 1.0 # Set alpha otherwise the marker is invisible self._marker.color.r = 0.0 self._marker.color.g = 0.0 self._marker.color.b = 1.0 # Indicate this target is planned self._marker_pub.publish(self._marker_topic, self._marker) self._marker_pub.publish(self._marker_topic, self._reference_marker) self._marker_pub.publish(self._marker_topic, self._local_target_marker) self._marker.action = Marker.MODIFY self._reference_marker.action = Marker.MODIFY self._reference_marker.color.a = 1.0 # Set alpha so it will become visible on next publish self._local_target_marker.action = Marker.MODIFY self._local_target_marker.color.a = 1.0 # Set alpha so it will become visible on next publish # Method to calculate the lookahead point given line segment from prior to target def calculateLineTwist(self, local_prior, local_target): # Define a line segment from prior to the target (assume 2D) pv = Vector3(local_target.point.x - local_prior.point.x, local_target.point.y - local_prior.point.y, 0.0) qv = Vector3(local_prior.point.x - self._current_position.point.x, local_prior.point.y - self._current_position.point.y, 0.0) # Find intersection of line segment with lookahead circle centered at the robot a = pv.x * pv.x + pv.y * pv.y # b = 2.0 * (qv.x * pv.x + qv.y * pv.y) c = (qv.x * qv.x + qv.y * qv.y) - self._lookahead * self._lookahead if (a < 0.001): Logger.logerr( ' %s Invalid prior and target for line: target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c)) self._return = 'failed' return None discrim = b * b - 4 * a * c if (discrim < 0.0): # No intersection - this should not be unless bad start or localization perturbation Logger.logwarn( ' %s No path recovery - no intersection for line: target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'failed' return None else: # solve quadratic equation for intersection points sqd = math.sqrt(discrim) t1 = (-b - sqd) / (2 * a) # min value t2 = (-b + sqd) / (2 * a) # max value if (t2 < t1): Logger.logwarn( ' %s Say what! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'failed' return None if (t2 < 0.0): # all intersections are behind the segment - this should not be unless bad start or localization perturbation if (t2 > -0.1): # likely due to localization perturbation Logger.logwarn( ' %s Circle is before segment - continue prior motion! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) return None else: Logger.logwarn( ' %s Circle is before segment! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'failed' return None elif (t1 > 1.0): # all intersections are past the segment Logger.loginfo( ' %s Circle is past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'done' return None elif (t1 < 0.0 and t2 > 1.0): # Segment is contained inside the lookahead circle Logger.loginfo( ' %s Circle contains segment - move along! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'done' return None elif (t2 > 1.0): # The lookahead circle extends beyond the target point - we are finished here Logger.loginfo( ' %s Circle extends past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'done' return None else: # This is the normal case # Must be line segment control = deepcopy(local_prior) control.point.x = control.point.x + t2 * pv.x # Control point in the odometry frame control.point.y = control.point.y + t2 * pv.y # Control point in the odometry frame control.point.z = control.point.z + t2 * pv.z # Control point in the odometry frame self._reference_marker.pose.position = control.point self._local_target_marker.pose.position = local_target.point control_robot = self.transformBody(control) curvature = 2.0 * control_robot.point.y / (self._lookahead * self._lookahead) self._twist.twist.angular.z = curvature * self._desired_velocity return control_robot return None
class DrivepathTestNew(EventState): ''' Lets the robot move along a given path. ># path PoseStamped[] Array of Positions of the robot. ># speed float Speed of the robot <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. ''' def __init__(self): ''' Constructor ''' super(DrivepathTestNew, self).__init__(outcomes=['reached', 'failed']) self._failed = False self._reached = False self._pathTopic = '/controller/path' self._pub = ProxyPublisher({self._pathTopic: MoveBaseActionPath}) def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._reached: return 'reached' def on_enter(self, userdata): self._path = MoveBaseActionPath() self._point = PoseStamped() self._point.pose.orientation.w = 1 self._point.pose.position.x = 0 self._path.goal.target_path.poses.append(self._point) Logger.loginfo('%(x).3f %(y).3f %(z).3f' % {'x': self._point.pose.orientation.x, 'y': self._point.pose.orientation.y, 'z': self._point.pose.orientation.z}) self._failed = False self._pub.publish(self._pathTopic, self._path) self._reached = True def on_stop(self): pass def on_exit(self, userdata): pass def on_pause(self): self._move_client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class DepthControl(EventState): ''' Example for a state to demonstrate which functionality is available for state implementation. This example lets the behavior wait until the given target_time has passed since the behavior has been started. -- target_time float Time which needs to have passed since the behavior started. <= continue Given time has passed. <= failed Example for a failure outcome. ''' def __init__(self, rate): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(DepthControl, self).__init__(outcomes=['finished', 'failed']) self._failed = False self.Kpw = 0.1 self.Vmax = 0.2 self.Vconst = 0.5 self.Wmax = 0.5 self.set_rate(rate) ## change the rate of 'execute' self.cmd_vel = '/cmd_vel' self.depth = '/soybot/center_camera/depth/image_raw' self._sub = ProxySubscriberCached({self.depth: Image}) self._pub = ProxyPublisher({self.cmd_vel: Twist}) self.bridge = CvBridge() ## so da para mudar rate por meio do GUI. Podemos pegar rosparam quando for rodar sem GUI. def normalize(self, data): data_n = (data - np.min(data)) / (np.max(data) - np.min(data)) * 255 return data_n def preprocessing(self, imagem): imagem_corrigida = deepcopy(imagem) imagem_corrigida.setflags(write=1) where_are_NaNs = np.isnan(imagem_corrigida) imagem_corrigida[where_are_NaNs] = 3.0 img = self.normalize(imagem_corrigida).astype(np.uint8) yf, xf = img.shape x = 150 img = img[200:yf, x:xf - x] #img = cv2.medianBlur(img,15) imga = deepcopy(img) img = cv2.createCLAHE(clipLimit=40.0, tileGridSize=(1, 640)).apply(img) #imagempp = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY,11,10) _, imagempp = cv2.threshold(img, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) a = np.sum(~imagempp[0:imagempp.shape[0] // 2, :]) #Logger.log('{}'.format(a), Logger.REPORT_HINT) if (a <= 2052240): # 4855200 para //5 end_row = 1 else: end_row = 0 return img, imagempp, imga, end_row def actuate(self, theta, found_setpoint): Vel = Twist() if (found_setpoint == 1): Vel.linear.x = self.Vconst Vel.angular.z = theta * self.Kpw if abs(Vel.angular.z) > self.Wmax: Vel.angular.z = np.sign(Vel.angular.z) * self.Wmax else: Vel.linear.x = 0 Vel.angular.z = 0 try: self._pub.publish(self.cmd_vel, Vel) except Exception as e: Logger.logwarn('Failed to send velocity:\n%s' % str(e)) self._failed = True def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._sub.has_msg(self.depth): data = self._sub.get_last_msg(self.depth) depth = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough") _, imagempp, _, end_row = self.preprocessing(depth) if (end_row): self.actuate(0, 0) return 'finished' frame, mask_show, centroid_1, mask_show2 = imp.get_centroid( ~imagempp, ~imagempp, ~imagempp, ~imagempp) y, x = imagempp.shape if (centroid_1[0] > 0): theta = (x / 2 - centroid_1[0]) theta = round(atan2(theta, y / 2), 2) found_setpoint = 1 else: found_setpoint = 0 self.actuate(theta, found_setpoint) if (self._failed): return 'failed' def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. # The following code is just for illustrating how the behavior logger works. # Text logged by the behavior logger is sent to the operator and displayed in the GUI. self.failed = False def on_exit(self, userdata): # This method is called when an outcome is returned and another state gets active. # It can be used to stop possibly running processes started by on_enter. pass # Nothing to do in this example. def on_start(self): # This method is called when the behavior is started. # If possible, it is generally better to initialize used resources in the constructor # because if anything failed, the behavior would not even be started. # In this example, we use this event to set the correct start time. #self._start_time = rospy.Time.now() pass def on_stop(self): # This method is called whenever the behavior stops execution, also if it is cancelled. # Use this event to clean up things like claimed resources. pass # Nothing to do in this example.
class EventState(OperatableState): """ A state that allows implementing certain events. """ def __init__(self, *args, **kwargs): super(EventState, self).__init__(*args, **kwargs) self._entering = True self._skipped = False self.__execute = self.execute self.execute = self._event_execute self._paused = False self._last_active_container = None self._feedback_topic = "flexbe/command_feedback" self._repeat_topic = "flexbe/command/repeat" self._pause_topic = "flexbe/command/pause" self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _event_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._pause_topic): msg = self._sub.get_last_msg(self._pause_topic) if msg.data: self._sub.remove_last_msg(self._pause_topic) rospy.loginfo("--> Pausing in state %s", self.name) self._pub.publish(self._feedback_topic, CommandFeedback(command="pause")) self._last_active_container = PriorityContainer.active_container PriorityContainer.active_container = "" self._paused = True if self._paused and not PreemptableState.preempt: self._notify_skipped() return self._loopback_name if self._entering: self._entering = False self.on_enter(*args, **kwargs) if self._skipped: self._skipped = False self.on_resume(*args, **kwargs) execute_outcome = self.__execute(*args, **kwargs) repeat = False if self._is_controlled and self._sub.has_msg(self._repeat_topic): rospy.loginfo("--> Repeating state %s", self.name) self._sub.remove_last_msg(self._repeat_topic) self._pub.publish(self._feedback_topic, CommandFeedback(command="repeat")) repeat = True if execute_outcome != self._loopback_name and not PreemptableState.switching or repeat: self._entering = True self.on_exit(*args, **kwargs) return execute_outcome def _notify_skipped(self): if not self._skipped: self.on_pause() self._skipped = True if self._is_controlled and self._sub.has_msg(self._pause_topic): msg = self._sub.get_last_msg(self._pause_topic) if not msg.data: self._sub.remove_last_msg(self._pause_topic) rospy.loginfo("--> Resuming in state %s", self.name) self._pub.publish(self._feedback_topic, CommandFeedback(command="resume")) PriorityContainer.active_container = self._last_active_container self._last_active_container = None self._paused = False super(EventState, self)._notify_skipped() def _enable_ros_control(self): super(EventState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._repeat_topic, Empty) self._sub.subscribe(self._pause_topic, Bool) def _disable_ros_control(self): super(EventState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._repeat_topic) self._sub.unsubscribe_topic(self._pause_topic) self._last_active_container = None if self._paused: PriorityContainer.active_container = None # Events # (just implement the ones you need) def on_start(self): """ Will be executed once when the behavior starts. """ pass def on_stop(self): """ Will be executed once when the behavior stops or is preempted. """ pass def on_pause(self): """ Will be executed each time this state is paused. """ pass def on_resume(self, userdata): """ Will be executed each time this state is resumed. """ pass def on_enter(self, userdata): """ Will be executed each time the state is entered from any other state (but not from itself). """ pass def on_exit(self, userdata): """ Will be executed each time the state will be left to any other state (but not to itself). """ pass
class SweetieBotRandHeadMovements(EventState): ''' Periodically reposition eyes and head of SweetieBot to random point using given FollowJointState controller. -- controller string FollowJointState controller name without `controller` prefix. -- duration float How long this state will be executed (seconds). -- interval float[2] Array of floats, maximal and minimal interval between movements. -- max2356 float[4] Max values for joint52, joint53, eyes_pitch, eyes_yaw. -- min2356 float[4] Min values for joint52, joint53, eyes_pitch, eyes_yaw. ># config dict Dictionary with keys 'duration', 'interval', 'max2356', 'min2356' to override default configuration. dict or key values can be set to None to use default value from parameters. <= done Finished. <= failed Failed to activate FollowJointState controller. ''' def __init__(self, controller='joint_state_head', duration=120, interval=[3, 5], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]): super(SweetieBotRandHeadMovements, self).__init__(outcomes=['done', 'failed'], input_keys=['config']) # Store topic parameter for later use. self._controller = 'motion/controller/' + controller # create proxies self._set_operational_caller = ProxyServiceCaller( {self._controller + '/set_operational': SetBool}) self._publisher = ProxyPublisher( {self._controller + '/in_joints_ref': JointState}) # state self._start_timestamp = None self._movement_timestamp = None self._movement_duration = Duration() # user input self.set_movement_parameters(duration, interval, min2356, max2356) # error in enter hook self._error = False def set_movement_parameters(self, duration, interval, min2356, max2356): if not isinstance(duration, (int, float)) or duration < 0: raise ValueError("Duration must be nonegative number.") if len(interval) != 2 or any([ not isinstance(t, (int, float)) for t in interval ]) or any([t < 0 for t in interval]) or interval[0] > interval[1]: raise ValueError("Interval must represent interval of time.") if len(min2356) != 4 or any( [not isinstance(v, (int, float)) for v in min2356]): raise ValueError("min2356: list of four numbers was expected.") if len(max2356) != 4 or any( [not isinstance(v, (int, float)) for v in max2356]): raise ValueError("max2356: list of four numbers was expected.") self._duration = Duration.from_sec(duration) self._interval = interval self._min2356 = min2356 self._max2356 = max2356 def on_enter(self, userdata): self._error = False # override configuration if necessary try: if userdata.config != None: # process dict duration = userdata.config.get("duration", self._duration.to_sec()) interval = userdata.config.get("interval", self._interval) min2356 = userdata.config.get("min2356", self._min2356) max2356 = userdata.config.get("max2356", self._max2356) # check parameters self.set_movement_parameters(duration, interval, min2356, max2356) except Exception as e: Logger.logwarn('Failed to process `config` input key:\n%s' % str(e)) self._error = True return # activate head controller try: res = self._set_operational_caller.call( self._controller + '/set_operational', True) except Exception as e: Logger.logwarn('Failed to activate `' + self._controller + '` controller:\n%s' % str(e)) self._error = True return if not res.success: Logger.logwarn('Failed to activate `' + self._controller + '` controller (SetBoolResponse: success = false).') self._error = True # set start timestamp self._start_timestamp = Time.now() self._movement_timestamp = Time.now() Logger.loginfo( 'Start random pose generation (eyes, head), controller `%s`.' % self._controller) def execute(self, userdata): if self._error: return 'failed' # check if time elasped if Time.now() - self._start_timestamp > self._duration: return 'done' # check if we have tosend new point if Time.now() - self._movement_timestamp > self._movement_duration: # determine new interval self._movement_timestamp = Time.now() self._movement_duration = Duration.from_sec( random.uniform(*self._interval)) # form message msg = JointState() msg.header = Header(stamp=Time.now()) msg.name = ['joint52', 'joint53', 'eyes_pitch', 'eyes_yaw'] # random durection x = random.uniform(0, 1) y = random.uniform(0, 1) # compute head position msg.position = [0, 0, 0, 0] msg.position[0] = self._min2356[0] * x + self._max2356[0] * (1 - x) msg.position[1] = self._min2356[1] * y + self._max2356[1] * (1 - y) # compute eyes position msg.position[2] = self._min2356[2] * y + self._max2356[2] * (1 - y) msg.position[3] = self._min2356[3] * x + self._max2356[3] * (1 - x) # send message try: self._publisher.publish(self._controller + '/in_joints_ref', msg) except Exception as e: Logger.logwarn('Failed to send JointState message `' + self._topic + '`:\n%s' % str(e)) def on_exit(self, userdata): try: res = self._set_operational_caller.call( self._controller + '/set_operational', False) except Exception as e: Logger.logwarn('Failed to deactivate `' + self._controller + '` controller:\n%s' % str(e)) return Logger.loginfo('Done random pose generation for eyes and head.')
class LockableState(ManuallyTransitionableState): """ A state that can be locked. When locked, no transition can be done regardless of the resulting outcome. However, if any outcome would be triggered, the outcome will be stored and the state won't be executed anymore until it is unlocked and the stored outcome is set. """ def __init__(self, *args, **kwargs): super(LockableState, self).__init__(*args, **kwargs) self._locked = False self._stored_outcome = None self.__execute = self.execute self.execute = self._lockable_execute self._feedback_topic = 'flexbe/command_feedback' self._lock_topic = 'flexbe/command/lock' self._unlock_topic = 'flexbe/command/unlock' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _lockable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._lock_topic): msg = self._sub.get_last_msg(self._lock_topic) self._sub.remove_last_msg(self._lock_topic) self._execute_lock(msg.data) if self._is_controlled and self._sub.has_msg(self._unlock_topic): msg = self._sub.get_last_msg(self._unlock_topic) self._sub.remove_last_msg(self._unlock_topic) self._execute_unlock(msg.data) if self._locked: if self._stored_outcome is None or self._stored_outcome == 'None': self._stored_outcome = self.__execute(*args, **kwargs) return None if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None': if self._parent.transition_allowed(self.name, self._stored_outcome): outcome = self._stored_outcome self._stored_outcome = None return outcome else: return None outcome = self.__execute(*args, **kwargs) if outcome is None or outcome == 'None': return None if not self._parent is None and not self._parent.transition_allowed(self.name, outcome): self._stored_outcome = outcome return None return outcome def _execute_lock(self, target): found_target = False if target == self._get_path(): found_target = True self._locked = True else: found_target = self._parent.lock(target) self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""])) if not found_target: rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path()) else: rospy.loginfo("--> Locking in state %s", target) def _execute_unlock(self, target): found_target = False if target == self._get_path(): found_target = True self._locked = False else: found_target = self._parent.unlock(target) self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""])) if not found_target: rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path()) else: rospy.loginfo("--> Unlocking in state %s", target) def _enable_ros_control(self): super(LockableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._lock_topic, String) self._sub.subscribe(self._unlock_topic, String) def _disable_ros_control(self): super(LockableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._lock_topic) self._sub.unsubscribe_topic(self._unlock_topic) def is_locked(self): return self._locked
class TurnState(EventState): ''' Turn state for a ground robot. This state allows the robot to turn at a certain angle at a specified velocity/ speed. -- t_speed float Speed at which to turn the robot -- turn_angle float The angle that the robot should make -- forward_dist float free distance in front of robot <= failed If behavior is unable to succeed on time <= done forward distance becomes sufficantly large ''' def __init__(self, t_speed, turn_angle, forward_dist, timeout): super(TurnState, self).__init__(outcomes=['failed', 'done']) self._t_speed = t_speed self._turn_angle = turn_angle self._forward_dist = forward_dist self._timeout = timeout self._start_time = None self.depth = None self.vel_topic = '/cmd_vel' self.scan_topic = '/depth_data' #create publisher passing it the vel_topic_name and msg_type self.pub = ProxyPublisher({self.vel_topic: Twist_float}) #create subscriber self.scan_sub = ProxySubscriberCached({self.scan_topic: Depth}) self.scan_sub.set_callback(self.scan_topic, self.scan_callback) def execute(self, userdata): if not self.cmd_pub: # check if message in self.cmd_pub to publish to /cmd_vel else we exit Logger.loginfo('messesage does not exist') return 'failed' #run obstacle checks [index 0: left, 360: middle, 719: right] if (self.depth is not None): Logger.loginfo('FWD free distance is: %s' % self.depth.center) Logger.loginfo('Turn angle is : %s' % self._turn_angle) if self.depth.center >= self._forward_dist: return 'done' #measure distance travelled elapsed_time = (rospy.Time.now() - self._start_time).to_sec() if elapsed_time >= self._timeout: Logger.loginfo('Reached timeout') return 'failed' #drive self.pub.publish(self.vel_topic, self.cmd_pub) def on_enter(self, userdata): Logger.loginfo("Turn RIGHT STARTED!") #set robot speed here self.cmd_pub = Twist_float() self.cmd_pub.vel = self._t_speed self.cmd_pub.angle = self._turn_angle self._start_time = rospy.Time.now() def on_exit(self, userdata): self.cmd_pub.vel = 0.0 self.cmd_pub.angle = 0.0 self.pub.publish(self.vel_topic, self.cmd_pub) Logger.loginfo("Turn RIGHT ENDED!") def on_start(self): Logger.loginfo("Drive FWD READY!") self._start_time = rospy.Time.now() #bug detected! (move to on_enter) def on_stop(self): Logger.loginfo("Turn RIGHT STOPPED!") def scan_callback(self, data): self.depth = data
class PrayingMantisCalibrationSM(Behavior): ''' A behavior that moves ATLAS into the "praying mantis" pose upon startup in order to get consistent joint encoder offsets for calibration purposes. ''' def __init__(self): super(PrayingMantisCalibrationSM, self).__init__() self.name = 'Praying Mantis Calibration' # parameters of this behavior # references to used behaviors # Additional initialization code can be added inside the following tags # [MANUAL_INIT] self._offset_topic = "/flor/controller/encoder_offsets" self._pub = ProxyPublisher({self._offset_topic: JointTrajectory}) self._joint_limits = AtlasDefinitions.arm_joint_limits # Define 90 percent positions for both arms (order of joints same as in _joint_names attribute) # atlas_v5 # - account for fall protection pads # - ignore the lower 3 joints, ie, the electric motor ones left_calib_upper = [ -1.4252, -1.4649, +0.1588, +2.2767, +0.1, +0.1, +0.1 ] left_calib_lower = [ +0.5470, +1.2355, +2.9297, +0.1191, -0.1, +1.0, -0.1 ] right_calib_upper = [ +1.4914, +1.4296, +0.2118, -2.2899, +0.1, +0.1, +0.1 ] right_calib_lower = [ -0.5470, -1.2355, +2.9297, -0.1191, -0.1, -1.0, -0.1 ] # # atlas_v5 (without shoulder pads) # left_calib_upper = [+0.5470, +1.2355, +2.9297, +2.1576, +0.1, +0.1, +0.1] # left_calib_lower = [-1.1869, -1.4296, +0.2118, +0.1191, -1.3, +1.0, -0.1] # right_calib_upper = [-0.5470, -1.2355, +2.9297, -2.1576, +0.1, +0.1, +0.1] # right_calib_lower = [+1.1869, +1.4296, +0.2118, -0.1191, -1.3, -1.0, -0.1] self._joint_calib = { 'left_arm': { 'upper': left_calib_upper, 'lower': left_calib_lower }, 'right_arm': { 'upper': right_calib_upper, 'lower': right_calib_lower } } self._joint_names = AtlasDefinitions.arm_joint_names # [/MANUAL_INIT] # Behavior comments: # O 47 211 /Perform_Checks/Manipulate_Limits # Without this output_key, Check Behavior complains. Because traj_past_limits could in theory be undefined during runtime. def create(self): initial_mode = "stand" motion_mode = "manipulate" mantis_mode = "manipulate_limits" percent_past_limits = 0.10 # before: 0.075 # x:788 y:72, x:474 y:133 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.target_limits = 'upper' _state_machine.userdata.cycle_counter = 1 _state_machine.userdata.stand_posture = None # calculated _state_machine.userdata.offsets = { 'left_arm': dict(), 'right_arm': dict() } # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._percent_past_limits = percent_past_limits # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory( ) # [/MANUAL_CREATE] # x:222 y:281, x:349 y:167 _sm_determine_offsets_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets']) with _sm_determine_offsets_0: # x:61 y:53 OperatableStateMachine.add( 'Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={ 'retrieved': 'Determine_Closest_Limits_Left', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'joint_positions': 'joint_positions'}) # x:319 y:54 OperatableStateMachine.add( 'Determine_Closest_Limits_Left', CalculationState(calculation=self.get_closest_limits_left), transitions={'done': 'Store_Offsets_Left'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_positions', 'output_value': 'joint_limits' }) # x:598 y:162 OperatableStateMachine.add( 'Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={ 'retrieved': 'Determine_Closest_Limits_Right', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'joint_positions': 'joint_positions'}) # x:584 y:275 OperatableStateMachine.add( 'Determine_Closest_Limits_Right', CalculationState(calculation=self.get_closest_limits_right), transitions={'done': 'Store_Offsets_Right'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_positions', 'output_value': 'joint_limits' }) # x:608 y:54 OperatableStateMachine.add( 'Store_Offsets_Left', FlexibleCalculationState( calculation=self.store_offsets_left, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'Get_Right_Joint_Positions'}, autonomy={'done': Autonomy.Off}, remapping={ 'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets' }) # x:340 y:274 OperatableStateMachine.add( 'Store_Offsets_Right', FlexibleCalculationState( calculation=self.store_offsets_right, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets' }) # x:528 y:401, x:707 y:282 _sm_manipulate_limits_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets', 'traj_past_limits']) with _sm_manipulate_limits_1: # x:100 y:156 OperatableStateMachine.add( 'Prevent_Runtime_Failure', CalculationState(calculation=lambda x: dict()), transitions={'done': 'Go_to_MANIPULATE_LIMITS'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'traj_past_limits' }) # x:387 y:55 OperatableStateMachine.add( 'Wait_for_Control_Mode_change', WaitState(wait_time=1.0), transitions={'done': 'Get_Left_Joint_Positions'}, autonomy={'done': Autonomy.Low}) # x:895 y:279 OperatableStateMachine.add( 'Gen_Traj_from_90%_to_110%', CalculationState(calculation=self.gen_traj_past_limits), transitions={'done': 'Go_to_110%_Joint_Limits'}, autonomy={'done': Autonomy.Low}, remapping={ 'input_value': 'current_joint_values', 'output_value': 'traj_past_limits' }) # x:893 y:391 OperatableStateMachine.add( 'Go_to_110%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=[ 'left_arm_traj_controller', 'right_arm_traj_controller' ]), transitions={ 'done': 'Determine_Offsets', 'failed': 'Determine_Offsets' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'trajectories': 'traj_past_limits'}) # x:651 y:385 OperatableStateMachine.add('Determine_Offsets', _sm_determine_offsets_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'offsets': 'offsets' }) # x:648 y:54 OperatableStateMachine.add( 'Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={ 'retrieved': 'Get_Right_Joint_Positions', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_positions': 'joint_positions_left'}) # x:904 y:53 OperatableStateMachine.add( 'Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={ 'retrieved': 'Generate_Joint_Positions_Struct', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_positions': 'joint_positions_right'}) # x:886 y:168 OperatableStateMachine.add( 'Generate_Joint_Positions_Struct', FlexibleCalculationState(calculation=lambda ik: { 'left_arm': ik[0], 'right_arm': ik[1] }, input_keys=['left', 'right']), transitions={'done': 'Gen_Traj_from_90%_to_110%'}, autonomy={'done': Autonomy.Off}, remapping={ 'left': 'joint_positions_left', 'right': 'joint_positions_right', 'output_value': 'current_joint_values' }) # x:92 y:55 OperatableStateMachine.add( 'Go_to_MANIPULATE_LIMITS', ChangeControlModeActionState(target_mode=mantis_mode), transitions={ 'changed': 'Wait_for_Control_Mode_change', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:574 y:247, x:276 y:549 _sm_update_calibration_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['offsets']) with _sm_update_calibration_2: # x:46 y:44 OperatableStateMachine.add( 'Process_Offsets', CalculationState(calculation=self.process_offsets), transitions={'done': 'Print_Offset_Info'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'offsets' }) # x:227 y:45 OperatableStateMachine.add( 'Print_Offset_Info', CalculationState(calculation=self.print_offset_info), transitions={'done': 'Publish_Offsets'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'none' }) # x:390 y:158 OperatableStateMachine.add( 'Ask_Perform_Update', OperatorDecisionState( outcomes=['update', 'no_update'], hint= "Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={ 'update': 'Convert_Offset_Data', 'no_update': 'finished' }, autonomy={ 'update': Autonomy.Full, 'no_update': Autonomy.Full }) # x:232 y:337 OperatableStateMachine.add( 'Update_Calibration', UpdateJointCalibrationState( joint_names=self._joint_names['left_arm'][0:4] + self._joint_names['right_arm'][0:4]), transitions={ 'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed' }, autonomy={ 'updated': Autonomy.Low, 'failed': Autonomy.High }, remapping={'joint_offsets': 'offset_list'}) # x:241 y:242 OperatableStateMachine.add( 'Convert_Offset_Data', CalculationState(calculation=lambda o: o['left_arm']['avg'] + o['right_arm']['avg']), transitions={'done': 'Update_Calibration'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'offset_list' }) # x:522 y:337 OperatableStateMachine.add( 'Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:246 y:445 OperatableStateMachine.add( 'Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:399 y:44 OperatableStateMachine.add( 'Publish_Offsets', CalculationState(calculation=self.publish_offsets), transitions={'done': 'Ask_Perform_Update'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'none' }) # x:978 y:197, x:394 y:80 _sm_perform_checks_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'target_limits', 'offsets'], output_keys=['cycle_counter', 'offsets']) with _sm_perform_checks_3: # x:105 y:74 OperatableStateMachine.add( 'Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Gen_Traj_to_90%_Limits', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:653 y:274 OperatableStateMachine.add('Manipulate_Limits', _sm_manipulate_limits_1, transitions={ 'finished': 'Gen_Traj_back_to_90%_Limits', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'offsets': 'offsets', 'traj_past_limits': 'traj_past_limits' }) # x:903 y:78 OperatableStateMachine.add( 'Increment_Cycle_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'cycle_counter' }) # x:344 y:277 OperatableStateMachine.add( 'Move_to_90%_Joint_Limits', MoveitStartingPointState(vel_scaling=0.3), transitions={ 'reached': 'Manipulate_Limits', 'failed': 'Move_to_90%_Joint_Limits' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'trajectories': 'trajectories_90'}) # x:114 y:276 OperatableStateMachine.add( 'Gen_Traj_to_90%_Limits', CalculationState(calculation=self.gen_traj_pre_limits), transitions={'done': 'Move_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'target_limits', 'output_value': 'trajectories_90' }) # x:636 y:78 OperatableStateMachine.add( 'Go_back_to_90%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=[ 'left_arm_traj_controller', 'right_arm_traj_controller' ]), transitions={ 'done': 'Increment_Cycle_counter', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'trajectories': 'traj_back_to_90'}) # x:636 y:172 OperatableStateMachine.add( 'Gen_Traj_back_to_90%_Limits', FlexibleCalculationState( calculation=self.gen_traj_back_from_limits, input_keys=['trajectories_90', 'traj_past_limits']), transitions={'done': 'Go_back_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={ 'trajectories_90': 'trajectories_90', 'traj_past_limits': 'traj_past_limits', 'output_value': 'traj_back_to_90' }) with _state_machine: # x:110 y:52 OperatableStateMachine.add( 'Initial_Control_Mode', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'Perform_Checks', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.High }) # x:712 y:317 OperatableStateMachine.add( 'Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'Update_Calibration', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:122 y:302 OperatableStateMachine.add('Perform_Checks', _sm_perform_checks_3, transitions={ 'finished': 'Are_We_Done_Yet?', 'failed': 'Intermediate_Mode_before_exit' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'target_limits': 'target_limits', 'offsets': 'offsets' }) # x:126 y:505 OperatableStateMachine.add( 'Are_We_Done_Yet?', DecisionState(outcomes=["done", "more"], conditions=lambda counter: "done" if counter >= 2 else "more"), transitions={ 'done': 'Intermediate_Mode_before_exit', 'more': 'Setup_next_Cycle' }, autonomy={ 'done': Autonomy.Low, 'more': Autonomy.High }, remapping={'input_value': 'cycle_counter'}) # x:15 y:404 OperatableStateMachine.add( 'Setup_next_Cycle', CalculationState(calculation=lambda lim: 'lower' if lim == 'upper' else 'upper'), transitions={'done': 'Perform_Checks'}, autonomy={'done': Autonomy.Low}, remapping={ 'input_value': 'target_limits', 'output_value': 'target_limits' }) # x:725 y:186 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'offsets': 'offsets'}) # x:726 y:427 OperatableStateMachine.add( 'Move_to_Stand_Posture', MoveitStartingPointState(vel_scaling=0.3), transitions={ 'reached': 'Initial_Mode_before_exit', 'failed': 'Move_to_Stand_Posture' }, autonomy={ 'reached': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'trajectories': 'stand_posture'}) # x:412 y:427 OperatableStateMachine.add( 'Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Move_to_Stand_Posture', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) return _state_machine # Private functions can be added inside the following tags # [MANUAL_FUNC] def gen_traj_pre_limits(self, limits_side): """Create trajectories for going to 90 percent of joint limits (either upper or lower limits)""" joint_config = { 'left_arm': self._joint_calib['left_arm'][limits_side], 'right_arm': self._joint_calib['right_arm'][limits_side] } return AtlasFunctions.gen_arm_trajectory_from_joint_configuration( joint_config) def _get_closest_limits(self, side, current_values): """ Selects the closest limit with respect to the current value (upper or lower bound). """ limits = self._joint_limits[side] closest_limit = list() for i in range(len(current_values)): near_limit = 'upper' if abs(limits['upper'][i] - current_values[i] ) < abs(limits['lower'][i] - current_values[i]) else 'lower' closest_limit.append(limits[near_limit][i]) rospy.loginfo("Limit joint positions: %s" % str(closest_limit)) rospy.loginfo("Current joint positions: %s" % str(current_values)) return closest_limit def get_closest_limits_left(self, current_values): return self._get_closest_limits('left_arm', current_values) def get_closest_limits_right(self, current_values): return self._get_closest_limits('right_arm', current_values) def gen_traj_past_limits(self, current_joint_values): """ Given all joint limits, generate a trajectory that takes the joints to 110%% percent past limits. atlas_v5 update: Do not push the lower 3 joints (electric ones) path the limits. """ result = dict() for arm in ['left_arm', 'right_arm']: current_values = current_joint_values[arm] arm_limits = self._get_closest_limits(arm, current_values) arm_target = list() arm_effort = list() percentage = self._percent_past_limits # Push the upper 4 joints against the limits for i in range(0, 4): near_limit = 'upper' if self._joint_limits[arm]['upper'][ i] == arm_limits[i] else 'lower' limit_range = self._joint_limits[arm]['upper'][ i] - self._joint_limits[arm]['lower'][i] offset_sign = 1 if near_limit is 'upper' else -1 arm_target.append(arm_limits[i] + offset_sign * percentage * limit_range) arm_effort.append(float(offset_sign)) # "Ignore" the lower 3 joints (electric motor ones) for i in range(4, 7): arm_target.append(current_values[i]) arm_effort.append( 0.0 ) # Zero effort stands for not applying additional force trajectory = JointTrajectory() trajectory.joint_names = self._joint_names[arm] point = JointTrajectoryPoint() point.positions = arm_target point.velocities = [0.0] * len( arm_target) # David's controller expects zero velocities point.effort = arm_effort point.time_from_start = rospy.Duration.from_sec(2.5) trajectory.points.append(point) # rospy.loginfo("110%% joint positions for %s arm: %s" % (arm, str(arm_target[0:4]))) # Only report the relevant joints result[arm] = trajectory return result def gen_traj_back_from_limits(self, input_keys): """The resulting trajectory points are the same as for going to 90%% of limits, but with the efforts set for David's controllers.""" traj_pre_limits = input_keys[0] traj_past_limits = input_keys[1] traj_back_to_90 = dict() for arm in ['left_arm', 'right_arm']: trajectory = traj_pre_limits[ arm] # Start with 90% of joint limits as the trajectory points trajectory.points[0].effort = traj_past_limits[arm].points[ 0].effort # Set efforts as per David's controllers trajectory.points[0].time_from_start = rospy.Duration.from_sec(1.0) # David's controller expects zero velocities trajectory.points[0].velocities = [0.0] * len( trajectory.points[0].positions) traj_back_to_90[arm] = trajectory return traj_back_to_90 def store_offsets(self, side, input_keys): limits = input_keys[0][0:4] # Ignore the lower 3 joints values = input_keys[1][0:4] # --//-- --//-- offsets = input_keys[2] counter = input_keys[3] offsets[side][counter] = [ limit - value for limit, value in zip(limits, values) ] msg = JointTrajectory() msg.joint_names = self._joint_names[side][ 0:4] # Ignore the lower 3 joints point = JointTrajectoryPoint() point.positions = values point.velocities = offsets[side][counter] msg.points.append(point) self._pub.publish(self._offset_topic, msg) Logger.loginfo("Publishing %s arm offsets to %s" % (side, self._offset_topic)) return offsets def publish_offsets(self, offsets, arms=['left_arm', 'right_arm'], current_values=[]): for side in arms: msg = JointTrajectory() msg.joint_names = self._joint_names[side] point = JointTrajectoryPoint() point.positions = current_values point.velocities = offsets[side]['avg'] msg.points.append(point) self._pub.publish(self._offset_topic, msg) Logger.loginfo("Publishing %s arm offsets to %s" % (side, self._offset_topic)) def store_offsets_left(self, input_keys): return self.store_offsets('left_arm', input_keys) def store_offsets_right(self, input_keys): return self.store_offsets('right_arm', input_keys) def process_offsets(self, offsets): for side in ['left_arm', 'right_arm']: # transposes list of lists from iteration,joint to joint,iteration iteration_values = map(list, zip(*offsets[side].values())) # Calculate the average offset and the deviation from the average offsets[side]['avg'] = [ sum(joint_entries) / float(len(joint_entries)) for joint_entries in iteration_values ] offsets[side]['diff'] = [ max(map(lambda x: abs(x - avg), joint_entries)) for joint_entries, avg in zip( iteration_values, offsets[side]['avg']) ] return offsets def print_offset_info(self, offsets): sides = ['left_arm', 'right_arm'] for side in sides: Logger.loginfo("Joint order (%s): %s" % (side, str(self._joint_names[side][0:4]))) rounded_offsets = [ round(offset, 3) for offset in offsets[side]['avg'] ] # round due to comms_bridge Logger.loginfo("Offsets (%s): %s" % (side, str(rounded_offsets))) # Logger.loginfo("Max deviation from average (%s): %s" % (side, str(offsets[side]['diff']))) pprint.pprint(offsets) # Pretty print to the "onboard" terminal
class WebsiteDummyModalState(EventState): ''' State to demonstrate functionality of webUI. Constantely publishes to /robot_status and also calls the service_proxy to change the shown webpage to the current robot status <= failed If service_proxy does not answer with success <= done If service_proxy answers with success ''' def __init__(self): super(WebsiteDummyModalState, self).__init__(outcomes=['done', 'failed']) #self.status_topic = '/web_service_proxy' self.status_topic = '/robot_status' self.website_service_proxy = '/web_service_proxy' self._wait_for_execution = True self.current_status_pub = ProxyPublisher( {self.status_topic: RobotStatus}) rospy.wait_for_service(self.website_service_proxy) self.client_website_proxy = rospy.ServiceProxy( self.website_service_proxy, WebServiceProxyMsg) def execute(self, userdata): self.current_status_pub.publish(self.status_topic, self.cmd_status) if self._response.success == True and (rospy.Time.now() - self._start_time).to_sec() > 10: Logger.loginfo("-------------------------") Logger.loginfo("successfull service_proxy: " + str(self._response.success)) Logger.loginfo(self._response.message) Logger.loginfo("data:" + self._response.data) Logger.loginfo("-------------------------") return 'done' elif self._response.success == False: return 'failed' def on_enter(self, userdata): self._start_time = rospy.Time.now() Logger.loginfo("started website_dummy_state1!") self.cmd_status = RobotStatus() self.cmd_status.currentTask = '192863' self.cmd_status.currentBehaviorStatus = '' self.cmd_status.lastTask = '203056' self.cmd_status.lastTaskResult = '' self.cmd_status.nextTask = '216385' self.cmd_status.allActions = ['296881', '296898', '297511'] self.cmd_status.indexCurrent = 2 self.cmd_status.actionResults = ['', 'failed'] self.cmd_status.batteryCapacity = 0.3 self.cmd_status.batteryVoltage = 0.0 self.cmd_status.listen = False service_call = WebServiceProxyMsg() service_call.command = "object" self._response = self.client_website_proxy.call(service_call.command) def on_exit(self, userdata): Logger.loginfo("exit started website_dummy_state1!") def on_start(self): Logger.loginfo("started website_dummy_state1 ready!") def on_stop(self): Logger.loginfo("started website_dummy_state1 STOPPED!") def scan_callback(self, data): self.data = data