def __init__(self, sentence, input_keys=[], emotion=0, block=True): """Constructor""" super(SaraSay, self).__init__(outcomes=['done'], input_keys=input_keys) if not (isinstance(sentence, types.LambdaType) and sentence.__name__ == "<lambda>") and len(input_keys) is not 0: raise ValueError( "In state " + type(self).__name__ + " saying \"" + sentence + "\", you need to define a lambda function for sentence.") # Get parameters self.msg = say() self.sentence = sentence self.emotion = UInt8() self.emotion.data = emotion self.block = block # Set topics self.emotion_topic = "sara_face/Emotion" self.say_topic = "/say" self.sayServiceTopic = "/wm_say" self.emotionPub = ProxyPublisher({self.emotion_topic: UInt8}) # Prepare wm_tts if self.block: self.sayClient = ProxyServiceCaller( {self.sayServiceTopic: say_service}) else: self.sayPub = ProxyPublisher({self.say_topic: say}) # Initialise the face sub self.emotionSub = ProxySubscriberCached({self.emotion_topic: UInt8}) self.lastEmotion = None
def test_publish_subscribe(self): t1 = '/pubsub_1' t2 = '/pubsub_2' pub = ProxyPublisher({t1: String}) pub = ProxyPublisher({t2: String}, _latch=True) sub = ProxySubscriberCached({t1: String}) self.assertTrue(pub.is_available(t1)) self.assertTrue(pub.wait_for_any(t1)) self.assertFalse(pub.wait_for_any(t2)) pub.publish(t1, String('1')) pub.publish(t2, String('2')) rospy.sleep( .5) # make sure latched message is sent before subscriber is added sub = ProxySubscriberCached({t2: String}) rospy.sleep( .5) # make sure latched message can be received before checking self.assertTrue(sub.has_msg(t1)) self.assertEqual(sub.get_last_msg(t1).data, '1') sub.remove_last_msg(t1) self.assertFalse(sub.has_msg(t1)) self.assertIsNone(sub.get_last_msg(t1)) self.assertTrue(sub.has_msg(t2)) self.assertEqual(sub.get_last_msg(t2).data, '2')
def __init__(self): super(TestPublisherState, self).__init__(outcomes=['done']) self.vel_topic = '/cmd_vel' self.status_topic = '/robot_status' #create publisher passing it the vel_topic_name and msg_type self.pub = ProxyPublisher({self.vel_topic: Twist}) self.current_status_pub = ProxyPublisher( {self.status_topic: RobotStatus})
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 __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, topic): """Constructor""" super(RiptideMoveState, self).__init__(outcomes=['Success', 'Failure'], input_keys=['type','args']) self._topic = topic self._pub = ProxyPublisher({self._topic: PoseStamped})
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 __init__(self): 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._struct_buffer = list() self._sync_lock = threading.Lock() self._outcome_topic = 'flexbe/mirror/outcome' # 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, controller='joint_state_head', duration=120, interval=[2.0, 4.0], joints=[], minimal=[], maximal=[]): super(RandJointsMovements, 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, joints, minimal, maximal) # error in enter hook self._error = False
def __init__(self, topic): """Constructor""" super(BigDawgsGottaEatState, self).__init__(outcomes=['done'], input_keys=['pose']) self._topic = topic self._pub = ProxyPublisher({self._topic: PoseStamped})
def __init__(self, hand): '''- Constructor ''' super(IRobotReleaseGraspState,self).__init__(outcomes=['done']) self._controller_topic = '/grasp_control/' + hand + '/release_grasp' self._pub = ProxyPublisher({self._controller_topic : GraspSelection})
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 __init__(self, target_time, distance, cmd_topic='/cmd_vel', odometry_topic='/odom'): super(MoveDistanceState, self).__init__(outcomes=['done', 'failed']) self._target_time = target_time self._distance = distance self._velocity = (distance / target_time) self._twist = TwistStamped() self._twist.twist.linear.x = self._velocity self._twist.twist.angular.z = 0 # 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._cmd_topic = cmd_topic self._pub = ProxyPublisher({self._cmd_topic: TwistStamped}) self._odom_topic = odometry_topic self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry}) self._starting_odom = None
def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(Arm_Control_State_GR, self).__init__(outcomes = ["continue", "failed"]) self._pub = ProxyPublisher({"sim2real/reset_status": Int32}) self._sub = ProxySubscriberCached({"sim2real/reset": Int32})
def __init__(self, target_time, velocity, rotation_rate, cmd_topic='/create_node/cmd_vel', sensor_topic='/create_node/sensor_state'): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(CreateTimedTwistState, self).__init__(outcomes=['done', 'failed']) # Store state parameter for later use. self._target_time = rospy.Duration(target_time) self._twist = TwistStamped() 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._done = None # Track the outcome so we can detect if transition is blocked self._sensor_topic = sensor_topic self._cmd_topic = cmd_topic self._sensor_sub = ProxySubscriberCached( {self._sensor_topic: CreateSensorState}) self._pub = ProxyPublisher({self._cmd_topic: TwistStamped})
def __init__(self, *args, **kwargs): super(RosState, self).__init__(*args, **kwargs) self._rate = rospy.Rate(10) self._is_controlled = False self._pub = ProxyPublisher() self._sub = ProxySubscriberCached()
def __init__(self, topic): """Constructor""" super(PublishTwistState, self).__init__(outcomes=['done'], input_keys=['twist']) self._topic = topic self._pub = ProxyPublisher({self._topic: Twist})
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, topic): ''' Constructor ''' super(PublisherEmptyState, self).__init__(outcomes=['done']) self._topic = topic self._pub = ProxyPublisher({self._topic: Empty})
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 __init__(self, topic='/meka/rosbagremote/record/named', pid=1): self._count = 1 self._pid = pid self._topic = topic self._pub = ProxyPublisher({topic: String}) super(RemoteRecord, self).__init__(outcomes=['done'], input_keys=['carrying'])
def __init__(self): super(SpeakState, self).__init__(outcomes=['done'], input_keys=['tts_text']) self.tts_topic = '/genie_tts' self.state_topic = '/genie_state' self.tts_pub = ProxyPublisher({self.tts_topic: String}) self.state_sub = ProxySubscriberCached({self.state_topic: UInt8})
def __init__(self, topic): ''' Constructor ''' super(PublisherStringState, self).__init__(outcomes=['done'], input_keys=['value']) self._topic = topic self._pub = ProxyPublisher({self._topic: String})
def __init__(self, ttsbg_text): super(TTSBulgarian, self).__init__(outcomes=['failed', 'done']) self._ttsbg_text_to_be_synthesized = ttsbg_text 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_ttsbg_text = ProxyPublisher( {self._ttsbg_text_to_be_synthesized_topic: String}) #create publisher passing it the ttsbg_topic 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})
def __init__(self, pubtopic, pubint): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(GenericPub, self).__init__(outcomes=["completed", "failed"]) # Store state parameters for later use. self._pubtopic = pubtopic self._pubint = pubint self._pub = ProxyPublisher({self._pubtopic: Int32})
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): '''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(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})
def __init__(self): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(START_Exploration_Transform, self).__init__(outcomes=['succeeded'], input_keys=['goalId'], output_keys=['goalId']) self._topic_explore = 'move_base/explore' self._pub = ProxyPublisher( {self._topic_explore: MoveBaseActionExplore})
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