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, 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, 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, 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,
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, 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
