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 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 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
def __set_pubSub(self): print("[Arm] name space : " + str(self.robot_name)) self.p2p_topic = str(self.robot_name) + '/p2p_pose_msg' self.__p2p_pub = ProxyPublisher({self.p2p_topic: P2PPose}) self.line_topic = str(self.robot_name) + '/kinematics_pose_msg' self.__line_pub = ProxyPublisher({self.line_topic: KinematicsPose}) self.arm_status_topic = str(self.robot_name) + '/status' self.__status_sub = ProxySubscriberCached( {self.arm_status_topic: StatusMsg})
def __init__(self, topic='motion/controller/look_at/in_pose_ref', duration=120, interval=[3, 5], maxXYZ=[1, 0.3, 0.5], minXYZ=[1.0, -0.3, 0.0], frame_xyz='base_link', frame_out='odom_combined'): super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure']) # Store topic parameter for later use. if not isinstance(topic, str): raise ValueError("Topic name must be string.") if not isinstance(duration, (int, float)) or duration <= 0: raise ValueError("Duration must be positive 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(minXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in minXYZ]): raise ValueError("minXYZ: list of three numbers was expected.") if len(maxXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in maxXYZ]): raise ValueError("maxXYZ: list of three numbers was expected.") if not isinstance(frame_xyz, str) or not isinstance(frame_out, str): raise ValueError("Frame names must be string.") self._topic = topic self._duration = Duration.from_sec(duration) self._interval = interval self._minXYZ = minXYZ self._maxXYZ = maxXYZ self._frame_in = frame_xyz self._frame_out = frame_out # create proxies self._publisher = ProxyPublisher({self._topic: PoseStamped}) self._tf_listener = ProxyTransformListener().listener() self._tf_listener.waitForTransform(self._frame_out, self._frame_in, rospy.Time(), rospy.Duration(1)) if not self._tf_listener.canTransform(self._frame_out, self._frame_in, rospy.Time(0)): raise ValueError( "Unable to perform transform between frames %s and %s." % (self._frame_out, self._frame_in)) # state self._start_timestamp = None self._movement_timestamp = None self._movement_duration = Duration() # error in enter hook self._error = False
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 __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 __set_pubSub(self): print("[Arm] name space : " + str(self.robot_name)) self.set_mode_topic = str(self.robot_name) + '/set_mode_msg' self.__set_mode_pub = ProxyPublisher({self.set_mode_topic: String}) self.joint_topic = str(self.robot_name) + '/joint_pose_msg' self.__joint_pub = ProxyPublisher({self.joint_topic: JointPose}) self.arm_status_topic = str(self.robot_name) + '/status' self.__status_sub = ProxySubscriberCached( {self.arm_status_topic: StatusMsg})
def __init__(self, mechanisms): super(mechanisms, self).__init__(outcomes = ['dropped']) topics = {'gripper hold': "arduino/close_gripper" , 'gripper release': "arduino/open_gripper", 'fire torpedo': "arduino/launch_torpedo", 'drop marker': "/arduino/open_dropper"} self._topic = topics[mechanism] (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._publisher = ProxyPublisher({self._topic: msg_type})
def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy): super(MirrorState, self).__init__(outcomes=given_outcomes) self.set_rate(100) self._target_name = target_name self._target_path = target_path self._outcome_topic = 'flexbe/mirror/outcome' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached({self._outcome_topic: UInt8})
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 __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 __init__(self, target_time, target_angle=360.0, cmd_topic='/create_node/cmd_vel', odometry_topic='/create_node/odom'): super(RotateAngleState, self).__init__(outcomes = ['done']) self._target_time = rospy.Duration(target_time) self._target_angle = target_angle*3.141593/180.0 self._twist = TwistStamped() self._twist.twist.linear.x = 0 self._twist.twist.angular.z = (self._target_angle / target_time) self._cmd_topic = cmd_topic self._pub = ProxyPublisher( {self._cmd_topic: TwistStamped}) self._start_time = None self._return = None # Track the outcome so we can detect if transition is blocked
class ManualState(EventState): ''' Project11 state where vehicle is operated from joystick commands. ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(ManualState, self).__init__(outcomes = ['autonomous', 'standby']) self.publishers = ProxyPublisher({'/helm':Helm}) self.p11sb = Project11StateBase() 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. js = self.p11sb.checkJoystick() if js is not None: requested_state = js[1] if requested_state is not None and requested_state != 'manual': return requested_state msg = js[0] helm = Helm() helm.header.stamp = rospy.Time.now() helm.throttle = msg.axes[1] helm.rudder = -msg.axes[3] self.publishers.publish('/helm',helm) 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. pass 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. 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.
def __init__(self, pubtopic, subtopic, pubint): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(GenericPubSub, self).__init__(outcomes=["completed", "failed"]) # Store state parameters for later use. self._rotation = None self._pubtopic = pubtopic self._subtopic = subtopic self._pubint = pubint self._pub = ProxyPublisher({self._pubtopic: Int32}) self._sub = ProxySubscriberCached({self._subtopic: Int32})
def __init__(self, target_time, square_side_in_m, speed): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(DriveInSquareByOdom, self).__init__(outcomes=['continue', 'failed']) self._target_time = rospy.Duration(target_time) self._square_side_lenght = square_side_in_m self._speed = speed self.vel_topic = '/cmd_vel' self.pub = ProxyPublisher({self.vel_topic: Twist}) # TODO set publish rate to 5 HZ self.finished = False
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
class RemoteRecordStop(EventState): ''' Stops rosbag recording. ''' def __init__(self, topic='/meka/rosbagremote/record/named'): self._topic = topic self._pub = ProxyPublisher({topic: String}) super(RemoteRecordStop, self).__init__(outcomes=['done']) def execute(self, d): Logger.loginfo('Stopping rosbag recording') self._pub.publish(self._topic, ':stop') return 'done'
def __init__(self, head_angle=25): """Constructor""" super(MoveBaseState, self).__init__(outcomes=['arrived', 'failed'], input_keys=['waypoint']) self._head_angle = head_angle self._head_topic = '/pepper_robot/head/pose' ProxyPublisher.createPublisher(self._pub, self._head_topic, String) self._action_topic = "/move_base" self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) self._arrived = False self._failed = False
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 __init__(self): super(WebsiteDummyState1, 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)
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 Reset_Control_State_GR(EventState): ''' Reset control takes in the trial information from Test control and on a succesful completion starts the Data Control. If all trials are completed then it will loop back to Test control. Direction is used for determining a successful and unsuccseful outcome for testing purposed but will need to be replaced with a different measure of success once it becomes more fleshed out. TODO: More complex information for trials, update info -- direction int TEMPORARY: Determines a succesful (1) or unsuccesful (0) outcome for testing purposes ># number_of_trials Trial information (currently just an int) <= continue All actions completed <= failed Trial control failed to initialize or call something TODO: Proper error checking <= completed All Trials have been succesfully completed, go back to Test control ''' def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Reset_Control_State_GR, self).__init__(outcomes = ["continue", "failed"], input_keys=["rotation"]) # Store state parameters for later use. self._rotation = None self._pub = ProxyPublisher({"reset_start": Int32}) self._sub = ProxySubscriberCached({"reset_complete": Int32}) #rospy.init_node('reset_control', anonymous=True) def execute(self, userdata): #publish to arduino #while(1): if self._sub.has_msg("reset_complete"): msg = self._sub.get_last_msg("reset_complete") print(msg) # in case you want to make sure the same message is not processed twice: self._sub.remove_last_msg("reset_complete") return "continue" def on_enter(self, userdata): #self._rotation = 5 #Initializes class variable from userdata, has to be done outside of constructor if(self._rotation is None and userdata.rotation is not None): self._rotation = userdata.rotation self._pub.publish("reset_start", self._rotation)
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
def __init__(self, *args, **kwargs): super(OperatableStateMachine, self).__init__(*args, **kwargs) self._message = None self._rate = rospy.Rate(10) self.id = None self.autonomy = None self._autonomy = {} self._ordered_states = [] self._pub = ProxyPublisher() self._sub = ProxySubscriberCached()
def __init__(self, topic, msg_type, value): super(PublisherState, self).__init__(outcomes=['done', 'failed']) # Store state parameter for later use. self._topic = topic # form message self._message = msg_type(**value) # create publisher self._publisher = ProxyPublisher({topic: msg_type}) # error in enter hook self._error = False
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
def __init__(self, person_stop_dist=0.5, with_j1=False, rate=10): super(AdjustTorso, self).__init__(outcomes = ['done']) self.with_j1 = bool(with_j1) self.rate = rate self.person_stop_dist = person_stop_dist self.MAX_VEL = 1.0 self.MIN_TRAJ_DUR = 0.5 self.SPEED_SCALE = 0.5 self.JOINT_NAMES = ['torso_j0', 'torso_j1'] self.TARGET_FRAME = 'panplate' self.LIMITS = { self.JOINT_NAMES[0]: [-0.75, 0.75], self.JOINT_NAMES[1]: [-0.1, 0.07], } self.j1_done = False self.people_pos = None self.js_pos = None self.js_pos_des = None self.t = tf.TransformListener(True, rospy.Duration(10)) self.PEOPLE_TOPIC = '/people_tracker/people' self.TORSO_STATE_TOPIC = '/meka_roscontrol/torso_position_trajectory_controller/state' self.TORSO_COMMAND_TOPIC = '/meka_roscontrol/torso_position_trajectory_controller/command' self.FACES_TOPIC = '/openface2/faces' sub_dict = { self.TORSO_STATE_TOPIC: JointTrajectoryControllerState, self.PEOPLE_TOPIC: People } if self.with_j1: from openface2_ros_msgs.msg import Faces sub_dict.update({ self.FACES_TOPIC: Faces }) self._subs = ProxySubscriberCached(sub_dict) self._pub = ProxyPublisher({self.TORSO_COMMAND_TOPIC: JointTrajectory}) frames = [] Logger.loginfo('waiting for transforms') while frames == []: frames = self.t.getFrameStrings()
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 TTSBulgarianFromIncommingKey(EventState): ''' TTS in Bulgarian language, sends messages trough MQTT to the Windows TTS VM console app. Sends a string to /ttsbg_mqtt/tts_text MQTT topic, trough sending a ROS String message to ROS topic /ttsbg_ros/tts_text mqtt_bridge resends the ROS message to the MQTT broker on /ttsbg_mqtt/tts_text. Default MQTT broker IP is 192.168.1.2 ># ttsbg_text String Incomming key - text to be synthesized <= failed If behavior is unable to send the TTS message <= done TTS message sent succesfully ''' def __init__(self): super(TTSBulgarianFromIncommingKey, self).__init__(input_keys=['ttsbg_text'], outcomes=['failed', 'done']) self._ttsbg_text_to_be_synthesized = None self._ttsbg_text_to_be_synthesized_topic = '/ttsbg_ros/tts_text' self._ttsbg_command_topic = '/ttsbg_ros/command' self._ttsbg_response_topic = '/ttsbg_ros/response' #create publisher passing it the ttsbg_topic and msg_type self.pub = ProxyPublisher({ self._ttsbg_text_to_be_synthesized_topic: String, self._ttsbg_command_topic: String }) # #create publisher passing it the ttsbg_command and msg_type # self.pub_ttsbg_command = ProxyPublisher({self._ttsbg_command_topic: String}) #create subscriber # self.sub_ttsbg_response = ProxySubscriberCached({self._ttsbg_response_topic: String}) self.sub = ProxySubscriberCached({self._ttsbg_response_topic: String}) def on_enter(self, userdata): self._ttsbg_text_to_be_synthesized = userdata.ttsbg_text def execute(self, userdata): # Logger.loginfo('V execute sam....') self.pub.publish(self._ttsbg_text_to_be_synthesized_topic, self._ttsbg_text_to_be_synthesized.decode('utf-8')) return 'done' def on_exit(self, userdata): pass def on_stop(self): pass
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 __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 __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 __init__(self): ''' Constructor ''' self._sm = None # 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 __init__(self, topic): """Constructor""" super(PublishPoseState, self).__init__(outcomes=['done'], input_keys=['pose']) self._topic = topic self._pub = ProxyPublisher({self._topic: PoseStamped})
def __init__(self): """ Constructor """ self._sm = None smach.set_loggers( rospy.logdebug, rospy.logwarn, rospy.logdebug, rospy.logerr # hide SMACH transition log spamming ) # 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)
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:])))
def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Drive_to_new, self).__init__(outcomes = ['succeeded'], input_keys = ['pose']) self._topic = '/move_base/simple_goal' self._pub = ProxyPublisher({self._topic: PoseStamped})
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 __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 __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 __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 __init__(self, useMoveBase=True): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Send_Request_new, self).__init__(outcomes = ['succeeded'], input_keys = ['goalId','params_distance','pose']) #self.userdata.goalId = 'none' #self.userdata.frameId = frameId 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 __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
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.
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})
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
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()
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})
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)
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 __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