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
