def __init__(self, robot, designator, max_duration): smach.State.__init__(self, outcomes=["ok", "timeout"]) self.robot = robot self.max_duration = rospy.Duration(max_duration) ds.check_resolve_type(designator, int) self.designator = designator
def __init__(self, robot, constraint_designator, breakout_designator, radius=.7, exclude_radius=0.3): """@param constraint_designator a Designator that resolves to the entity to explore @param breakout_designator when this Designator successfully resolves, the state signals it is done. For example, it could resolve to an item of a class you are looking for.""" super(NavigateToExplore, self).__init__(robot) self.robot = robot check_resolve_type( constraint_designator, ed_msgs.msg.EntityInfo ) #Check that the constraint designator resolves to an Entity self.constraint_designator = constraint_designator check_resolve_type( breakout_designator, ed_msgs.msg.EntityInfo ) #Check that the constraint designator resolves to an Entity self.breakout_designator = breakout_designator self.radius = radius self.exclude_radius = exclude_radius self.visited_list = []
def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of", threshold=0.0): """ Constructor :param robot: robot object :param segmented_entity_ids_designator: designator that is used to store the segmented objects :param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of :param threshold: float for classification score. Entities whose classification score is lower are ignored (i.e. are not added to the segmented_entity_ids_designator) """ smach.State.__init__(self, outcomes=["done"]) self.robot = robot self.threshold = threshold ds.check_resolve_type(entity_to_inspect_designator, Entity) self.entity_to_inspect_designator = entity_to_inspect_designator if isinstance(segmentation_area, str): self.segmentation_area_des = ds.VariableDesignator(segmentation_area) else: # ds.check_resolve_type(segmentation_area, "str") self.segmentation_area_des = segmentation_area ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, counter, limit): smach.State.__init__(self, outcomes=['counted', 'limit_reached']) self.limit = limit ds.check_resolve_type(counter, int) ds.is_writeable(counter) self.counter = counter
def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of"): """ Constructor :param robot: robot object :param segmented_entity_ids_designator: designator that is used to store the segmented objects :param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of """ smach.State.__init__(self, outcomes=["done"]) self.robot = robot ds.check_resolve_type(entity_to_inspect_designator, Entity) self.entity_to_inspect_designator = entity_to_inspect_designator if isinstance(segmentation_area, str): self.segmentation_area_des = ds.VariableDesignator( segmentation_area) else: # ds.check_resolve_type(segmentation_area, "str") self.segmentation_area_des = segmentation_area ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, robot, entity_designator, radius = .7): """@param entity_designator resolves to the entity the robot should observe""" super(NavigateToObserve, self).__init__(robot) self.robot = robot check_resolve_type(entity_designator, ed.msg.EntityInfo) #Check that the entity_designator resolves to an Entity self.entity_designator = entity_designator self.radius = radius
def __init__(self, entity_designator, namespace="designator"): smach.State.__init__(self, outcomes=['succeeded', 'failed']) ds.check_resolve_type(entity_designator, EntityInfo) self.entity_designator = entity_designator self.namespace = namespace self.publisher = rospy.Publisher("executive_markers", Marker)
def __init__(self, results_designator, item_designator): smach.State.__init__(self, outcomes=['done']) ds.is_writeable(results_designator) ds.check_resolve_type(results_designator, dict) self.results = results_designator ds.check_resolve_type(item_designator, str) self.item = item_designator
def __init__(self, robot, person_name="", name_designator=None, n_samples=5): smach.State.__init__(self, outcomes=['succeeded_learning', 'failed_learning', 'timeout_learning']) self.robot = robot self.person_name = person_name if name_designator: ds.check_resolve_type(name_designator, str) self.name_designator = name_designator self.n_samples = n_samples
def __init__(self, robot, entity_designator, arm_designator=None): super(NavigateToGrasp, self).__init__(robot, reset_head=False) self.robot = robot check_resolve_type(entity_designator, ed.msg.EntityInfo) #Check that the entity_designator resolves to an Entity self.entity_designator = entity_designator check_resolve_type(arm_designator, Arm) #Check that the arm_designator resolves to an Arm self.arm_designator = arm_designator if not arm_designator: rospy.logerr('NavigateToGrasp: side should be determined by entity_designator. Please specify left or right, will default to left') self.arm_designator = Designator(robot.leftArm)
def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of"): smach.State.__init__(self, outcomes=["done"]) self.robot = robot ds.check_resolve_type(entity_to_inspect_designator, EntityInfo) self.entity_to_inspect_designator = entity_to_inspect_designator self.segmentation_area = segmentation_area ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, robot, door_entity_designator, direction, name=None): """ :param robot The robot with which to query ED :param door_entity_designator Designator resolving to an entity of type door that has a push_start_waypoint and push_destination_waypoint data field :param direction 'start' or 'destination' to select one of the two data fields :param name Name for this designator to aid in debugging. """ super(WaypointOfDoorDesignator, self).__init__(resolve_type=EntityInfo, name=name) self.robot = robot ds.check_resolve_type(door_entity_designator, EntityInfo) self.door_entity_designator = door_entity_designator self.data_field = {'start':'push_start_waypoint', 'destination':'push_destination_waypoint'}[direction]
def __init__(self, robot, entity_designator, radius=0.7): """ :param robot: (Robot) object :param entity_designator: EdEntityDesignator for the object to observe :param radius: (float) desired distance to the pose of the entity """ super(NavigateToObserve, self).__init__(robot) self.robot = robot check_resolve_type(entity_designator, Entity) # Check that the entity_designator resolves to an Entity self.entity_designator = entity_designator self.radius = radius
def __init__(self, robot, waypoint_designator, radius = 0.15, look_at_designator=None, speak=True): """@param waypoint_designator resolves to a waypoint stored in ED""" super(NavigateToWaypoint, self).__init__(robot, speak=speak) self.robot = robot check_resolve_type(waypoint_designator, Entity) #Check that the waypoint_designator resolves to an Entity if look_at_designator is not None: check_resolve_type(look_at_designator, Entity) #Check that the look_at_designator resolves to an Entity self.waypoint_designator = waypoint_designator self.radius = radius self.look_at_designator = look_at_designator
def __init__(self, robot, entity_designator, arm_designator=None): super(NavigateToGrasp, self).__init__(robot, reset_head=False) self.robot = robot check_resolve_type(entity_designator, Entity) # Check that the entity_designator resolves to an Entity self.entity_designator = entity_designator check_resolve_type(arm_designator, PublicArm) # Check that the arm_designator resolves to an Arm self.arm_designator = arm_designator if not arm_designator: rospy.logerr('NavigateToGrasp: side should be determined by entity_designator.\ Please specify left or right, will default to left. This is Deprecated') self.arm_designator = UnoccupiedArmDesignator(self.robot, {})
def __init__(self, robot, constraint_designator, breakout_designator, radius = .7, exclude_radius = 0.3): """@param constraint_designator a Designator that resolves to the entity to explore @param breakout_designator when this Designator successfully resolves, the state signals it is done. For example, it could resolve to an item of a class you are looking for.""" super(NavigateToExplore, self).__init__(robot) self.robot = robot check_resolve_type(constraint_designator, ed.msg.EntityInfo) #Check that the constraint designator resolves to an Entity self.constraint_designator = constraint_designator check_resolve_type(breakout_designator, ed.msg.EntityInfo) #Check that the constraint designator resolves to an Entity self.breakout_designator = breakout_designator self.radius = radius self.exclude_radius = exclude_radius self.visited_list = []
def __init__(self, robot, entity_designator_area_name_map, entity_lookat_designator): super(NavigateToSymbolic, self).__init__(robot) self.robot = robot #Check that the entity_designator_area_name_map's keys all resolve to EntityInfo's assert (all( entity_desig.resolve_type == ed.msg.EntityInfo for entity_desig in entity_designator_area_name_map.keys())) self.entity_designator_area_name_map = entity_designator_area_name_map check_resolve_type( entity_lookat_designator, ed.msg.EntityInfo ) #Check that the entity_designator resolves to an Entity self.entity_lookat_designator = entity_lookat_designator
def __init__(self, robot, person_name="", name_designator=None, nr_tries=5): """ Constructor :param robot: robot object :param person_name: string indicating the name that will be given :param name_designator: designator returning a string with the name of the person. This will be used if no person name is provided :param nr_tries: maximum number of tries """ smach.State.__init__(self, outcomes=["succeeded", "failed"]) self._robot = robot self._person_name = person_name if name_designator: ds.check_resolve_type(name_designator, str) self._name_designator = name_designator self._nr_tries = nr_tries
def __init__(self, robot, place_pose_designator, arm_designator=None): """Navigate so that the arm can reach the place point @param place_pose_designator designator that resolves to a geometry_msgs.msg.PoseStamped @param arm which arm to eventually place with? """ super(NavigateToPlace, self).__init__(robot) self.robot = robot # Check that place_pose_designator actually returns a PoseStamped check_resolve_type(place_pose_designator, FrameStamped) self.place_pose_designator = place_pose_designator self.arm_designator = arm_designator if not arm_designator: rospy.logerr('NavigateToPlace: side should be determined by arm_designator.' 'Please specify left or right, will default to left') self.arm_designator = Designator(robot.leftArm)
def __init__(self, robot, spec_designator, speech_result_designator, timeout=10, look_at_standing_person=True): smach.State.__init__(self, outcomes=["heard", "no_result"]) self.robot = robot ds.check_resolve_type(spec_designator, str) ds.check_resolve_type(speech_result_designator, HMIResult) ds.is_writeable(speech_result_designator) self.spec_designator = spec_designator self.speech_result_designator = speech_result_designator self.timeout = timeout self.look_at_standing_person = look_at_standing_person
def __init__(self, robot, entity_designator, radius=0.7, margin=0.075): """ :param robot: (Robot) object :param entity_designator: EdEntityDesignator for the object to observe :param radius: (float) desired distance to the pose of the entity [m] :param margin: (float) allowed margin w.r.t. specified radius on both sides [m] """ super(NavigateToObserve, self).__init__(robot) self.robot = robot check_resolve_type( entity_designator, Entity) # Check that the entity_designator resolves to an Entity self.entity_designator = entity_designator self.radius = radius self.margin = margin
def __init__(self, robot, place_pose_designator, arm_designator=None): """Navigate so that the arm can reach the place point @param place_pose_designator designator that resolves to a geometry_msgs.msg.PoseStamped @param arm which arm to eventually place with? """ super(NavigateToPlace, self).__init__(robot) self.robot = robot # Check that place_pose_designator actually returns a PoseStamped check_resolve_type(place_pose_designator, FrameStamped) self.place_pose_designator = place_pose_designator self.arm_designator = arm_designator if not arm_designator: rospy.logerr( 'NavigateToPlace: side should be determined by arm_designator.' 'Please specify left or right, will default to left') self.arm_designator = Designator(robot.leftArm)
def __init__(self, robot, entity_designator_area_name_map, entity_lookat_designator): """ Constructor :param robot: robot object :param entity_designator_area_name_map: dictionary mapping EdEntityDesignators to a string or designator resolving to a string, representing the area, e.g., entity_designator_area_name_map[<EdEntity>] = 'in_front_of'. :param entity_lookat_designator: EdEntityDesignator defining the entity the robot should look at. This is used to compute the orientation constraint. """ super(NavigateToSymbolic, self).__init__(robot) self.robot = robot # Check that the entity_designator_area_name_map's keys all resolve to EntityInfo's assert(all(entity_desig.resolve_type == Entity for entity_desig in entity_designator_area_name_map.keys())) self.entity_designator_area_name_map = entity_designator_area_name_map check_resolve_type(entity_lookat_designator, Entity) # Check that the entity_designator resolves to an Entity self.entity_lookat_designator = entity_lookat_designator
def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of", unknown_threshold=0.0, filter_threshold=0.0): """ Constructor :param robot: robot object :param segmented_entity_ids_designator: designator that is used to store the segmented objects :param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of :param unknown_threshold: Entities whose classification score is lower than this float are not marked with a type :param filter_threshold: Entities whose classification score is lower than this float are ignored (i.e. are not added to the segmented_entity_ids_designator) """ smach.State.__init__(self, outcomes=["done"]) self.robot = robot self.unknown_threshold = unknown_threshold self.filter_threshold = filter_threshold ds.check_resolve_type(entity_to_inspect_designator, Entity) self.entity_to_inspect_designator = entity_to_inspect_designator ds.check_type(segmentation_area, str) if isinstance(segmentation_area, str): self.segmentation_area_des = ds.VariableDesignator( segmentation_area) elif isinstance(segmentation_area, ds.Designator): self.segmentation_area_des = segmentation_area else: raise RuntimeError( "This shouldn't happen. Wrong types should have raised an exception earlier" ) ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, robot, spec_designator, choices_designator, speech_result_designator, time_out=rospy.Duration(10), look_at_standing_person=True): smach.State.__init__(self, outcomes=["heard", "no_result"]) self.robot = robot ds.check_resolve_type(spec_designator, str) ds.check_resolve_type(choices_designator, dict) ds.check_resolve_type(speech_result_designator, GetSpeechResponse) ds.is_writeable(speech_result_designator) self.spec_designator = spec_designator self.choices_designator = choices_designator self.speech_result_designator = speech_result_designator self.time_out = time_out self.look_at_standing_person = look_at_standing_person
def __init__(self, results): smach.State.__init__(self, outcomes=['all_succeeded','partly_succeeded','all_failed']) ds.check_resolve_type(results, dict) self.results_designator = results