def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'dispatch_plan',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'dispatch_plan')
self.planner_interface = PlannerInterface()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'report',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed'],
input_keys=['command'])
self.timeout = kwargs.get('timeout', 120.)
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'verify_person',
save_sm_state=save_sm_state,
outcomes=[
'no_empty_spot', 'new_person',
'already_logged_person', 'known_person',
'face_not_seen'
],
output_keys=['destination_locations'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'verify_person')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.total_locations = kwargs.get('total_locations', 0)
self.person_recognition_threshold = kwargs.get(
'person_recognition_threshold', 0)
self.retry_count = 0
self.timeout = kwargs.get('timeout', 10.)
self.bye = ['bye', 'by', 'buy', 'bi']
occupied_locations = self.kb_interface.get_obj_instance(
'occupied_locations', DomainFormula._type)
if occupied_locations is None:
self.kb_interface.insert_obj_instance('occupied_locations',
DomainFormula())
# wait for listen action server
self.listen_client = actionlib.SimpleActionClient(
'listen_server', ListenAction)
listen_wait_result = self.listen_client.wait_for_server(
timeout=rospy.Duration(self.timeout))
if not listen_wait_result:
raise RuntimeError('Failed to wait for "listen_server" action')
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'receive_information',
save_sm_state=save_sm_state,
input_keys=[],
output_keys=['target_entity'],
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'receive_information')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.timeout = kwargs.get('timeout', 25)
self.threshold = kwargs.get('threshold', 0.68)
# Load the rasa model
model_directory = get_package_path('rasa_nlu_models', 'common',
'where_is_this', 'nlu')
self.interpreter = Interpreter.load(model_directory)
# wait for listen action server
self.listen_client = actionlib.SimpleActionClient(
"listen_server", ListenAction)
listen_wait_result = self.listen_client.wait_for_server(
timeout=rospy.Duration(self.timeout))
if not listen_wait_result:
raise RuntimeError('failed to wait for "listen_server" action')
# Speech Matcher
self.speech_matcher = SpeechMatching()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'select_storage_location',
save_sm_state=save_sm_state,
input_keys=['grasped_object'],
output_keys=['destination_locations', 'storage_location'],
outcomes=['succeeded'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'select_storage_location')
self.storage_locations_file = kwargs.get('storage_locations_file',
'storage_location.yaml')
self.available_trays = kwargs.get('available_trays',
['tray_1', 'tray_2'])
self.tray_locations = kwargs.get('tray_locations', {
'tray_1': 'long_table_a_trays',
'tray_2': 'long_table_a_trays'
})
self.available_bins = kwargs.get('available_bins', ['bin_a', 'bin_b'])
self.bin_locations = kwargs.get('bin_locations', {
'bin_a': 'bin_a',
'bin_b': 'bin_b'
})
self.default_storage_location = kwargs.get('default_storage_location',
'bin')
storage_locations_path = get_package_path('mdr_wrs_tidy_up', 'config',
self.storage_locations_file)
if not self.storage_locations_file:
raise ValueError(
'[{0}] Argument "storage_locations_file" cannot be empty'.
format(self.state_name))
self.storage_locations_map = load_yaml_file(storage_locations_path)
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'check_mates_to_find',
save_sm_state=save_sm_state,
outcomes=['mates_left', 'no_mates_left'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'check_mates_to_find')
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'_receive_object',
save_sm_state=save_sm_state,
outcomes=[
'succeeded', 'failed', 'failed_after_retrying',
'person_not_found'
],
input_keys=['person_name', 'object_to_transport'],
output_keys=['grasped_object'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'get_person_pose')
self.action_server = kwargs.get('action_server',
'receive_object_server')
self.timeout = kwargs.get('timeout', 120.)
self.context_aware = kwargs.get('context_aware', True)
self.reception_detection = kwargs.get('reception_detection', True)
self.posture_ratio_ranges = kwargs.get('posture_ratio_ranges', {})
self.person_name = kwargs.get('person_name', None)
self.object_to_transport = kwargs.get('object_to_transport', None)
self.object_category = kwargs.get('object_category', None)
self.gripper_controller_pkg = kwargs.get('gripper_controller_pkg',
'mdr_gripper_controller')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
self.client = actionlib.SimpleActionClient(self.action_server,
ReceiveObjectAction)
self.client.wait_for_server(rospy.Duration(10.))
gripper_controller_class = getattr(
import_module(self.gripper_controller_pkg), 'GripperController')
self.gripper_controller = gripper_controller_class()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'get_user_data',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed', 'failed_after_retrying'],
input_keys=['destination_locations'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'get_user_data')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
self.timeout = kwargs.get('timeout', 120.)
self._sheet_id = kwargs.get('sheet_id', '')
self._worksheet_name = kwargs.get('worksheet_name', 'responses')
self._num_known_entries = None
self._loop_rate_s = kwargs.get('loop_rate_s', 2.)
data = self._load_spreadsheet()
if data:
self._num_known_entries = data.shape[0]
rospy.loginfo(
"[get_user_data] Initialized the user data query with {0} prior known users"
.format(self._num_known_entries))
else:
rospy.logerr(
"[get_user_data] Could not initialize user data sheet!")
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'describe_location',
save_sm_state=save_sm_state,
input_keys=['target_entity'],
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'describe_location')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.pose_timeout = kwargs.get('pose_timeout', 5.)
self.pose_topic_name = kwargs.get('pose_topic_name', '/amcl_pose')
self.retry_count = 0
self.current_pose = None
self.current_top_pos = None
rospy.loginfo(
'[describe_location] Waiting for topological_position server')
self.topological_position_client = rospy.ServiceProxy(
'topological_position', TopologicalPosition)
rospy.loginfo('[describe_location] topological_position server up')
rospy.loginfo(
'[describe_location] Waiting for topological_path_plan server')
self.topological_path_client = rospy.ServiceProxy(
'topological_path_plan', TopologicalPath)
rospy.loginfo('[describe_location] topological_path_plan server up')
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'store_groceries_goal_selector',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'store_groceries_goal_selector')
self.number_of_shelves = kwargs.get('number_of_shelves', -1)
self.planner_interface = PlannerInterface()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'say_sentence',
save_sm_state=save_sm_state,
outcomes=['succeeded'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'say_sentence')
self.sentences = list(kwargs.get('sentences', list()))
self.sleep_time = kwargs.get('sleep_time', -1.)
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'describe_person',
save_sm_state=save_sm_state,
input_keys=['person_name'],
outcomes=['succeeded'])
self.bridge = CvBridge()
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'describe_person')
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'store_person',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'retake_picture', 'failed'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'store_person')
self.person_msg_id = kwargs.get('person_msg_id', 'person_0')
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'say',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed'])
self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.))
self.sentence = kwargs.get('sentence', '')
self.sound_topic = kwargs.get('sound_topic', '/say')
self.waiting_time_sec = kwargs.get('waiting_time_sec', 5.)
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'find_people',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'find_people')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
self.timeout = 120.
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'open_door',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'waiting'],
output_keys=['command'])
self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.))
self.start_time = rospy.Time.now()
self.restart_state = False
self.asked_for_door_opening = False
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'identify_posture',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed'],
input_keys=['person_list'],
output_keys=['posture', 'person_pose'])
self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
self.posture_height_width_ratio_ranges = kwargs.get(
'posture_height_width_ratio_ranges', {})
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'select_object_for_picking',
save_sm_state=save_sm_state,
input_keys=['detected_objects'],
output_keys=['selected_object'],
outcomes=['succeeded'])
self.tf_listener = tf.TransformListener()
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'select_object_for_picking')
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'wait_for_name',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed'])
self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.))
self.speech_topic = kwargs.get('speech_topic', '/recognized_speech')
self.robot_name = kwargs.get('name', 'bot')
self.cmd_received = False
self.speech_sub = rospy.Subscriber(self.speech_topic, String,
self.get_speech_cmd)
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'place',
save_sm_state=save_sm_state,
input_keys=['grasped_object'],
outcomes=['pick_new_object', 'finished',
'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries')
self.state_name = kwargs.get('state_name', 'place')
self.timeout = kwargs.get('timeout', 120.)
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'listen',
save_sm_state=save_sm_state,
output_keys=['listen_result'],
outcomes=['received_command', 'failed'])
self.listen_server = kwargs.get('listen_server',
'/mdr_actions/listen_server')
self.listen_client = actionlib.SimpleActionClient(
self.listen_server, ListenAction)
self.listen_client.wait_for_server()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'perceive_plane',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'perceive_planes')
self.timeout = kwargs.get('timeout', 120.)
self.plane_prefix = kwargs.get('plane_prefix', 0)
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'detect_person',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed', 'failed_after_retrying'],
output_keys=['person_list'])
self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over')
self.action_server = kwargs.get('action_server', 'find_people_server')
self.timeout = kwargs.get('timeout', 120.)
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
self.client = actionlib.SimpleActionClient(self.action_server, FindPeopleAction)
self.client.wait_for_server(rospy.Duration(10.))
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'find_objects',
save_sm_state=save_sm_state,
outcomes=[
'succeeded', 'no_objects', 'failed',
'failed_after_retrying'
],
input_keys=[
'environment_objects',
'floor_objects_cleared',
'table_objects_cleared',
'object_location',
'destination_locations'
],
output_keys=[
'detected_objects',
'floor_objects_cleared',
'table_objects_cleared'
])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'find_objects')
self.object_detection_timeout_s = kwargs.get(
'object_detection_timeout', 5.)
self.object_detection_server_name = kwargs.get(
'object_detection_server_name', '/mas_perception/detect_objects')
self.cloud_obstacle_detection_topic = kwargs.get(
'cloud_obstacle_detection_topic',
'/mas_perception/cloud_obstacle_detection/obstacle_objects')
self.cloud_obstacle_cache_reset_topic = kwargs.get(
'cloud_obstacle_cache_reset_topic',
'/mas_perception/cloud_obstacle_detection/reset_cache')
self.max_allowed_obj_height_cm = kwargs.get(
'max_allowed_obj_height_cm', 0.15)
self.min_allowed_dist_to_leg = kwargs.get('min_allowed_dist_to_leg',
0.15)
self.tables_to_clean_up = kwargs.get('tables_to_clean_up',
['long_table_b', 'tall_table'])
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.large_object_size_threshold = kwargs.get(
'large_object_size_threshold', 0.15)
self.large_object_height_threshold = kwargs.get(
'large_object_height_threshold', 0.075)
self.large_object_aspect_ratio_threshold = kwargs.get(
'large_object_aspect_ratio_threshold', 0.4)
self.filtered_poses_topic = kwargs.get('filtered_poses_topic',
'filtered_object_poses')
self.filtered_poses_pub = rospy.Publisher(self.filtered_poses_topic,
PoseArray,
queue_size=1)
self.__init_ros_components()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'throw',
save_sm_state=save_sm_state,
input_keys=['grasped_object'],
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'throw')
self.throwing_target_name = kwargs.get('throwing_target_name', '')
self.timeout = kwargs.get('timeout', 120.)
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'place',
save_sm_state=save_sm_state,
input_keys=['grasped_object'],
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'place_based_on_category')
self.placing_surface_prefix = kwargs.get('placing_surface_prefix', '')
self.timeout = kwargs.get('timeout', 120.)
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self,
'describe_people',
save_sm_state=save_sm_state,
outcomes=[
'succeeded', 'failed',
'no_image_received',
'failed_after_retrying'
])
self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.))
self.no_detection_waiting_time = kwargs.get(
'no_detection_waiting_time', 5.)
self.image_topic = kwargs.get('image_topic', '/image')
self.detect_person_server = kwargs.get(
'detect_person_server', '/mdr_actions/detect_person_server')
self.recognize_emotion_server = kwargs.get(
'recognize_emotion_server',
'/mdr_actions/recognize_emotion_server')
self.recognize_gender_server = kwargs.get(
'recognize_gender_server',
'/mdr_actions/gender_recognition_server')
self.sound_topic = kwargs.get('sound_topic', '/say')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
self.image_sub = rospy.Subscriber(self.image_topic, Image,
self.get_image)
self.image_received = False
self.image = None
self.detect_person_client = actionlib.SimpleActionClient(
self.detect_person_server, DetectPersonAction)
rospy.loginfo('Waiting for %s' % self.detect_person_server)
self.detect_person_client.wait_for_server()
self.recognize_emotion_client = actionlib.SimpleActionClient(
self.recognize_emotion_server, RecognizeEmotionAction)
rospy.loginfo('Waiting for %s' % self.recognize_emotion_server)
self.recognize_emotion_client.wait_for_server()
self.recognize_gender_client = actionlib.SimpleActionClient(
self.recognize_gender_server, GenderRecognitionAction)
rospy.loginfo('Waiting for %s' % self.recognize_gender_server)
self.recognize_gender_client.wait_for_server()
rospy.loginfo('Starting people description')
self.start_time = rospy.Time.now()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'listen',
save_sm_state=save_sm_state,
input_keys=['listen_result'],
outcomes=['start_store_groceries', 'continue_waiting'])
self.start_command = kwargs.get('start_command', 'store groceries')
self.process_command_server = kwargs.get(
'process_command_server',
'/mdr_actions/process_speech_command_server')
self.process_command_client = actionlib.SimpleActionClient(
self.process_command_server, ProcessSpeechAction)
self.process_command_client.wait_for_server()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'release_object',
save_sm_state=save_sm_state,
input_keys=['grasped_object',
'environment_objects'],
outcomes=['succeeded', 'failed',
'failed_after_retrying'])
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'release_object')
self.action_server_name = kwargs.get('action_server_name', 'place_server')
self.release_timeout = kwargs.get('release_timeout', 120.)
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.release_target_name = kwargs.get('release_target_name', 'bin_a')
self.retry_count = 0
self.tf_listener = tf.TransformListener()
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(self, 'detect_person',
save_sm_state=save_sm_state,
input_keys=[],
output_keys=[],
outcomes=['succeeded', 'failed', 'failed_after_retrying'])
# Get parameters
self.sm_id = kwargs.get('sm_id', '')
self.state_name = kwargs.get('state_name', 'detect_person')
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.timeout = kwargs.get('timeout', 30) # not used right now
self.person_in_front = False
self.person_status_sub = rospy.Subscriber(DetectPerson.DOOR_STATUS_TOPIC, Bool, self.update_person_in_front)