def __init__(self, robot, arm):
StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=['position'])
@cb_interface(outcomes=['succeeded'])
def prepare_grasp(ud):
# Open gripper (non-blocking)
arm.resolve().send_gripper_goal('open', timeout=0)
# Torso up (non-blocking)
robot.torso.high()
# Arm to position in a safe way
arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0)
# Open gripper
arm.resolve().send_gripper_goal('open', timeout=0.0)
return 'succeeded'
with self:
StateMachine.add("PREPARE_GRASP", CBState(prepare_grasp),
transitions={'succeeded': 'SIMPLE_NAVIGATE_TO_GRASP'})
StateMachine.add("SIMPLE_NAVIGATE_TO_GRASP", SimpleNavigateToGrasp(robot, arm),
transitions={'unreachable': 'failed',
'arrived': 'SIMPLE_PICKUP',
'goal_not_defined': 'failed'})
StateMachine.add("SIMPLE_PICKUP", SimplePickup(robot, arm),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id)
with self:
StateMachine.add("NAVIGATE_BACK_TO_DISHWASHER",
NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
transitions={'arrived': 'CUSTOM_PLACE',
'unreachable': 'SAY_DISHWASHER_NOT_REACHABLE',
'goal_not_defined': 'SAY_DISHWASHER_NOT_REACHABLE'})
StateMachine.add('SAY_DISHWASHER_NOT_REACHABLE', Say(robot, [
"I cannot reach the dishwasher, let me try again in a couple of seconds! Hopefully this will go right this time."],
block=True),
transitions={'spoken': 'NAVIGATE_BACK_TO_DISHWASHER2'})
StateMachine.add("NAVIGATE_BACK_TO_DISHWASHER2",
NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
transitions={'arrived': 'CUSTOM_PLACE',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add('CUSTOM_PLACE', CustomPlace(robot),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
source_entity_designator = EdEntityDesignator(robot, id=source_entity)
navigation_area_designator1 = VariableDesignator('in_front_of')
navigation_area_designator2 = VariableDesignator('in_back_of')
with self:
StateMachine.add("NAVIGATE_TO_INSPECT",
NavigateToSymbolic(robot, {source_entity_designator: navigation_area_designator1},
source_entity_designator),
transitions={'arrived': 'SIMPLE_GRASP',
'unreachable': 'NAVIGATE_TO_INSPECT2',
'goal_not_defined': 'NAVIGATE_TO_INSPECT2'})
StateMachine.add('SIMPLE_GRASP', FindAndGrab(robot),
transitions={'succeeded': 'succeeded',
'failed': 'NAVIGATE_TO_INSPECT2'})
StateMachine.add("NAVIGATE_TO_INSPECT2",
NavigateToSymbolic(robot, {source_entity_designator: navigation_area_designator2},
source_entity_designator),
transitions={'arrived': 'SIMPLE_GRASP2',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add('SIMPLE_GRASP2', FindAndGrab(robot),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, startPose='StandInit'):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])
with self:
StateMachine.add('ENABLE_STIFF', EnableStiffnessState(), transitions={'succeeded': 'START_POSITION'})
StateMachine.add('START_POSITION', GoToPostureState(startPose, 0.5), transitions={'succeeded': 'succeeded'})
def __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])
with self:
StateMachine.add('CROUCH_POSE', GoToPostureState('Crouch', 0.5), transitions={'succeeded': 'DISABLE_STIFF'})
StateMachine.add('DISABLE_STIFF', DisableStiffnessState(), transitions={'succeeded': 'succeeded'})
def __init__(self):
StateMachine.__init__(
self,
outcomes=['standby', 'exit', 'aborted', 'preempted'],
input_keys=['control_infos'])
self.register_termination_cb(self.set_timer)
with self:
StateMachine.add('CONTROL_MANAGER',
ControlManager(),
transitions={
'recharge': 'RECHARGE',
'assisted_teleop': 'ASSISTED_TELEOP',
'semi_autonomous': 'SEMI_AUTONOMOUS',
'standby': 'standby',
'exit': 'exit'
})
StateMachine.add('RECHARGE',
Recharge(),
transitions={'succeeded': 'standby'})
StateMachine.add('ASSISTED_TELEOP', AssistedTeleop())
StateMachine.add('SEMI_AUTONOMOUS',
SimpleActionState('move_base',
MoveBaseAction,
input_keys=['control_infos'],
goal_cb=self.move_base_goal_cb),
transitions={'succeeded': 'standby'})
def __init__(self):
StateMachine.__init__(
self,
outcomes=['succeeded', 'standby', 'aborted', 'preempted'],
input_keys=['mode_change_infos'],
output_keys=['mode_infos'])
self.userdata.mode_infos = {}
with self:
# ===== MODE_MANAGER State =====
StateMachine.add('MODE_MANAGER',
ModeManager(),
transitions={
'patrol': 'PATROL',
'assistance': 'ASSISTANCE',
'standby': 'standby'
})
# ===== PATROL State =====
StateMachine.add('PATROL',
Patrol(),
transitions={
'succeeded': 'succeeded',
'next_mission': 'PATROL'
})
# ===== MANUAL State =====
StateMachine.add('ASSISTANCE',
Assistance(),
transitions={
'exit': 'succeeded',
'restart': 'ASSISTANCE'
})
def __init__(self):
StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted', 'next_mission'],
input_keys=['mode_infos'],
output_keys=['log_mission'])
self.userdata.log_mission = {}
self.register_termination_cb(self.save_mission_msg)
with self:
StateMachine.add('PATROL_MANAGER',
PatrolManager(),
transitions={
'navigation': 'NAVIGATION',
'pause': 'PAUSE'
})
StateMachine.add('NAVIGATION',
Navigation(),
transitions={'succeeded': 'PAUSE'})
StateMachine.add('PAUSE',
Pause(),
transitions={'succeeded': 'next_mission'})
def __init__(self):
StateMachine.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'])
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
pose = Pose()
pose.position = Point(8.2, -3.0, 0.0)
pose.orientation = Quaternion(0.0, 0.0, 1.0, 0.0)
nav_goal.target_pose.pose = pose
self.nav_docking_station = SimpleActionState('move_base',
MoveBaseAction,
goal=nav_goal)
with self:
StateMachine.add('NAVIGATE_TO_OUTLET',
self.nav_docking_station,
transitions={
'succeeded': 'DOCKING',
'aborted': 'NAVIGATE_TO_OUTLET'
})
StateMachine.add('DOCKING',
ServiceState(
'battery_simulator/set_battery_level',
SetBatteryLevel, 100),
transitions={'succeeded': 'succeeded'})
def __init__(self, robot, trashbin_id, radius, yaw_offset):
StateMachine.__init__(self, outcomes=['done'])
@cb_interface(outcomes=['done'])
def control_to_pose(_):
trash_bin_frame = robot.ed.get_entity(trashbin_id).pose.frame
trash_bin_position = trash_bin_frame.p
base_frame = robot.base.get_location().frame
base_position = base_frame.p
trash_bin_to_base_vector = base_position - trash_bin_position
trash_bin_to_base_vector.Normalize()
desired_base_position = trash_bin_position + radius * trash_bin_to_base_vector
goal_pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id="map"),
pose=Pose(position=Point(x=desired_base_position.x(),
y=desired_base_position.y()),
orientation=Quaternion(*quaternion_from_euler(
0, 0,
math.atan2(trash_bin_to_base_vector.y(),
trash_bin_to_base_vector.x()) -
math.pi + yaw_offset))))
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02,
0.2)).execute({})
return 'done'
with self:
self.add('CONTROL_TO_TRASH_BIN',
CBState(control_to_pose),
transitions={'done': 'done'})
def __init__(self, timeout=10):
sm.__init__(self,
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'],
input_keys=['detect_person_goal'],
output_keys=['detect_person_feedback',
'detect_person_result'])
detection_model_path = rospy.get_param('~config_file', '')
image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw')
with self:
sm.add('SETUP_DETECT_PERSON', SetupDetectPerson(),
transitions={'succeeded': 'DETECT_PERSON',
'failed': 'SETUP_DETECT_PERSON'})
sm.add('DETECT_PERSON',
DetectPerson(image_topic=image_topic,
detection_model_path=detection_model_path),
transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'SET_ACTION_LIB_FAILED'})
sm.add('SET_ACTION_LIB_FAILED',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
sm.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
def __init__(self, robot, cupboard_id, cupboard_navigation_area,
required_items):
StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
output_keys=["item_picked"])
cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
with self:
StateMachine.add(
"NAVIGATE_TO_CUPBOARD",
NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area},
cupboard),
transitions={
'arrived': 'PICK_ITEM_FROM_CUPBOARD',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
StateMachine.add("PICK_ITEM_FROM_CUPBOARD",
PickItemFromCupboardDrawer(
robot, cupboard_id, required_items),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot, item):
StateMachine.__init__(self, outcomes=['found', 'not_found'])
source_entity_designator = EdEntityDesignator(robot, id=source_entity)
# description_designator = VariableDesignator({
# 'type': 'cup'
# })
area_name_designator = VariableDesignator('on_top_of')
navigation_area_designator1 = VariableDesignator('in_back_of')
navigation_area_designator2 = VariableDesignator('in_front_of')
with self:
StateMachine.add('FIND', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator1, item),
transitions={'found': 'found',
'not_found': 'FIND2'})
StateMachine.add('FIND2', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator2, item),
transitions={'found': 'found',
'not_found': 'FIND3'})
StateMachine.add('FIND3', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator1, item),
transitions={'found': 'found',
'not_found': 'FIND4'})
StateMachine.add('FIND4', CustomFind(robot, source_entity_designator, area_name_designator,
navigation_area_designator2, item),
transitions={'found': 'found',
'not_found': 'not_found'})
def __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['text'], output_keys=['text'])
with self:
StateMachine.add('START_LISTEN',
StartRecognitionState(),
transitions={'succeeded': 'ANSWER_DETECTION'}
)
StateMachine.add('ANSWER_DETECTION',
GetRecognizedWordNoEmptyList(timeout=30),
transitions={'succeeded': 'STOP_LISTEN',
'timeouted': 'STOP_LISTEN_ABORTED'},
remapping={'recognized_words': 'text'}
)
StateMachine.add('STOP_LISTEN',
StopRecognitionState(),
transitions={'succeeded': 'GET_SINGLE_RESPONSE'}
)
StateMachine.add('GET_SINGLE_RESPONSE',
CBState(self.list_cb, input_keys=['text'], output_keys=['text'], outcomes=['succeeded', 'aborted']),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'}
)
StateMachine.add('STOP_LISTEN_ABORTED',
StopRecognitionState(),
transitions={'succeeded': 'aborted'}
)
def __init__(self,
robot,
table_id,
table_navigation_area,
table_close_navigation_area,
placement_height=0.7):
StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
input_keys=["item_picked"])
table = EdEntityDesignator(robot=robot, id=table_id)
with self:
# StateMachine.add("NAVIGATE_TO_TABLE",
# NavigateToSymbolic(robot, {table: table_navigation_area}, table),
# transitions={'arrived': 'NAVIGATE_TO_TABLE_CLOSE',
# 'unreachable': 'failed',
# 'goal_not_defined': 'failed'})
StateMachine.add(
"NAVIGATE_TO_TABLE_CLOSE",
NavigateToSymbolic(robot, {table: table_close_navigation_area},
table),
transitions={
'arrived': 'PLACE_ITEM_ON_TABLE',
'unreachable': 'SAY_PICK_AWAY_THE_CHAIR',
'goal_not_defined': 'failed'
})
StateMachine.add("FORCE_DRIVE",
ForceDrive(robot, -0.1, 0, 0, 5.0),
transitions={'done': 'SAY_PICK_AWAY_THE_CHAIR'})
StateMachine.add(
"SAY_PICK_AWAY_THE_CHAIR",
Say(
robot,
"Please pick away the chair, I cannot get close enough to the {}"
.format(table_id)),
transitions={'spoken': 'WAIT_FOR_PICK_AWAY_CHAIR'})
StateMachine.add('WAIT_FOR_PICK_AWAY_CHAIR',
WaitTime(robot, 5),
transitions={
'waited': 'SAY_THANKS',
'preempted': 'failed'
})
StateMachine.add('SAY_THANKS',
Say(robot, "Thank you darling"),
transitions={'spoken': 'NAVIGATE_TO_TABLE_CLOSE'})
StateMachine.add("PLACE_ITEM_ON_TABLE",
PlaceItemOnTable(robot, table_id,
placement_height),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
StateMachine.add("FIND_CUP",
CustomFindCup(robot),
transitions={'succeeded': 'succeeded',
'failed': 'FIND_CUP'})
def __init__(self, action_req, action_server):
StateMachine.__init__(self, outcomes=['done', 'failed'])
# align and approach cart state params
offset_to_approach_pose_m = float(
rospy.get_param('~offset_to_approach_pose_m', '0.55'))
backward_vel_docking_ms = float(
rospy.get_param('~backward_vel_docking_ms', '0.1'))
max_rot_vel_docking_rads = float(
rospy.get_param('~max_rot_vel_docking_rads', '0.1'))
approach_x_thresh_m = float(
rospy.get_param('~approach_x_thresh_m', '0.05'))
approach_y_thresh_m = float(
rospy.get_param('~approach_y_thresh_m', '0.05'))
approach_yaw_thresh_rad = float(
rospy.get_param('~approach_yaw_thresh_rad', '0.1'))
# couple to cart state params
max_coupling_attempts = int(
rospy.get_param('max_coupling_attempts', '3'))
self.userdata.action_req = action_req
self.userdata.action_server = action_server
with self:
StateMachine.add(
'ALIGN_AND_APPROACH_CART',
AlignAndApproachCart(
offset_to_approach_pose_m=offset_to_approach_pose_m,
backward_vel_docking_ms=backward_vel_docking_ms,
max_rot_vel_docking_rads=max_rot_vel_docking_rads,
approach_x_thresh_m=approach_x_thresh_m,
approach_y_thresh_m=approach_y_thresh_m,
approach_yaw_thresh_rad=approach_yaw_thresh_rad),
transitions={
'approach_succeeded': 'UNCOUPLE_FROM_CART',
'cart_not_found': 'failed',
'cart_pose_publisher_not_available': 'failed',
'timeout': 'ALIGN_AND_APPROACH_CART'
})
StateMachine.add('UNCOUPLE_FROM_CART',
UncoupleFromCart(),
transitions={
'uncoupling_succeeded': 'COUPLE_TO_CART',
'uncoupling_failed': 'COUPLE_TO_CART',
'cannot_switch_to_robot_mode':
'COUPLE_TO_CART'
})
StateMachine.add(
'COUPLE_TO_CART',
CoupleToCart(max_coupling_attempts=max_coupling_attempts),
transitions={
'coupling_succeeded': 'done',
'coupling_failed': 'ALIGN_AND_APPROACH_CART',
'cannot_switch_to_load_mode': 'failed'
})
def __init__(self, angle=-math.pi/6):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], output_keys=['square'])
self.turns = 0
self.angle = angle
self.has_spoken = False
with self:
StateMachine.add('FIND_SQUARE1', ReadTopicSquare(), transitions={'succeeded': 'SPEAK_F', 'aborted': 'CHECK_SPEAK'})
text = 'I have not found the marker. I will look around'
StateMachine.add('SPEAK_NF', SpeechState(text=text, blocking=False), transitions={'succeeded': 'LOOK_DOWN'})
text = 'I have found the marker!'
StateMachine.add('SPEAK_F', SpeechState(text=text, blocking=True), transitions={'succeeded': 'LOOK_FRONT'})
def check_turn(ud):
if (self.turns == 1):
self.angle = -self.angle
_angle = self.angle
self.turns += 1
elif (self.turns == 2):
self.angle = -self.angle
self.turns = 0
_angle = 0
return 'go_back'
else:
_angle = self.angle
self.turns += 1
ud.joint_angles = [_angle]
return 'succeeded'
StateMachine.add('CHECK_TURN', CBState(check_turn, outcomes=['succeeded', 'go_back'], output_keys=['joint_angles']),
transitions={'succeeded': 'LOOK_MIDDLE', 'go_back': 'PRE_GO_BACK'}, remapping={'joint_angles': 'joint_angles'})
StateMachine.add('LOOK_MIDDLE', JointAngleState(['HeadPitch'], [0.0]), transitions={'succeeded': 'LOOK_AROUND'})
StateMachine.add('LOOK_AROUND', JointAngleState(['HeadYaw']), transitions={'succeeded': 'FIND_SQUARE1'})
StateMachine.add('LOOK_DOWN', JointAngleState(['HeadPitch'], [0.5]), transitions={'succeeded': 'FIND_SQUARE2'})
StateMachine.add('FIND_SQUARE2', ReadTopicSquare(), transitions={'succeeded': 'SPEAK_F', 'aborted': 'CHECK_TURN'})
StateMachine.add('LOOK_FRONT', JointAngleState(['HeadYaw'], [0.0]), transitions={'succeeded': 'succeeded'})
StateMachine.add('PRE_GO_BACK', JointAngleState(['HeadPitch', 'HeadYaw'], [0.0, 0.0]), transitions={'succeeded': 'GO_BACK'})
StateMachine.add('GO_BACK', MoveToState(Pose2D(-0.20, 0.0, 0.0)), transitions={'succeeded': 'FIND_SQUARE1'})
def check_speak(ud):
if self.has_spoken:
return 'no_speak'
self.has_spoken = True
return 'speak'
StateMachine.add('CHECK_SPEAK', CBState(check_speak, outcomes=['speak', 'no_speak']), transitions={'speak': 'SPEAK_NF', 'no_speak': 'LOOK_DOWN'})
def __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])
with self:
StateMachine.add('STEP_LEFT', MoveToState(objective=Pose2D(0.0, 0.10, 0.0)), transitions={'succeeded': 'SAY_TURNTABLE'})
text = 'I will check the table to look for a tomato'
StateMachine.add('SAY_TURNTABLE', SpeechState(text=text, blocking=False), transitions={'succeeded': 'TURN_TO_TABLE'})
StateMachine.add('TURN_TO_TABLE', MoveToState(objective=Pose2D(0.0, 0.0, TURN_TO_TABLE_ANGLE)), transitions={'succeeded': 'LOOK_AT_TABLE'})
StateMachine.add('LOOK_AT_TABLE', JointAngleState(['HeadPitch', 'RElbowRoll', 'LElbowRoll'], [0.15, 0.05, -0.05]), transitions={'succeeded': 'DISABLE_ARM_WALK'})
StateMachine.add('DISABLE_ARM_WALK', SetArmsWalkingState(leftArmEnabled=False, rightArmEnabled=False),
transitions={'succeeded': 'SCAN_TABLE'})
StateMachine.add('SCAN_TABLE', ScanTable(), transitions={'succeeded': 'LOOK_AT_SQUARE'})
StateMachine.add('LOOK_AT_SQUARE', JointAngleState(['HeadPitch'], [0.5]), transitions={'succeeded': 'READ_SQUARE'})
StateMachine.add('READ_SQUARE', ReadTopicSquare(),
transitions={'succeeded': 'PREPARE_APPROACH','aborted':'APPROACH_TABLE_HARD'}, remapping={'square': 'square'})
def put_approach_obj(ud):
#transf_square = transform_pose(Pose2D(ud.square.z, ud.square.x, 0.0))
ud.objective = Pose2D(max(abs(ud.square.z)-OFFSET_TABLE, 0.0), 0.0, 0.0)
#raw_input( '******************' + str(Pose2D(max(abs(transf_square.x)-OFFSET_TABLE, 0.0), 0.0, 0.0)) )
return 'succeeded'
StateMachine.add('PREPARE_APPROACH', CBState(put_approach_obj, input_keys=['square'], output_keys=['objective'], outcomes=['succeeded']),
transitions={'succeeded':'APPROACH_TABLE'}, remapping={'objective': 'objective'})
StateMachine.add('APPROACH_TABLE', MoveToState(),
transitions={'succeeded': 'LOOK_A_LITTLE_DOWN'})
StateMachine.add('LOOK_A_LITTLE_DOWN', JointAngleState(['HeadPitch'], [0.12]), transitions={'succeeded': 'SAY_GRASP'})
StateMachine.add('APPROACH_TABLE_HARD', MoveToState(objective=Pose2D(APPROACH_TABLE_DIST, 0.0, 0.0)),
transitions={'succeeded': 'SAY_GRASP'})
text = 'Look this is a tomato! I will try to grasp it!'
StateMachine.add('SAY_GRASP', SpeechState(text=text, blocking=False), transitions={'succeeded': 'GRASP_TOMATO'})
StateMachine.add('GRASP_TOMATO', ExecuteBehavior(behavior_name='tomato_grasp'), transitions={'succeeded':'SAY_GO_TO_RELEASE'})
text = 'I am going to release it in the pan! I am already hungry!'
StateMachine.add('SAY_GO_TO_RELEASE', SpeechState(text=text, blocking=False), transitions={'succeeded': 'GO_TO_PAN'})
StateMachine.add('GO_TO_PAN', MoveToState(objective=Pose2D(0.0, DISTANCE_TO_PAN, 0.0)), transitions={'succeeded': 'RELEASE_TOMATO'})
StateMachine.add('RELEASE_TOMATO', ExecuteBehavior(behavior_name='tomato_release'), transitions={'succeeded':'SAY_FINISH'})
text = 'I am done, now we just have to wait until it is cooked.'
StateMachine.add('SAY_FINISH', SpeechState(text=text, blocking=False), transitions={'succeeded': 'succeeded'})
def __init__(self, helper_obj):
StateMachine.__init__(self,
outcomes=['complete', 'preempted', 'aborted'],
input_keys=['mission_data'],
output_keys=['mission_status'])
self.__helper_obj = helper_obj
with self:
# This state will prepare for the mission
StateMachine.add('PREPARE_MISSION',
PrepareMission4(self.__helper_obj),
transitions={
'ready': 'DEFAULT_HEAD_POSITION',
'error': 'complete'
})
# Set up action goal for deafult head position
default_position_pan, default_position_tilt = self.__helper_obj.CameraDefaultPos(
)
head_goal = point_headGoal()
head_goal.absolute = True
head_goal.pan = default_position_pan
head_goal.tilt = default_position_tilt
# Add the default camera position state. Which moves the head to the default position
StateMachine.add('DEFAULT_HEAD_POSITION',
SimpleActionState(
'head_control_node',
point_headAction,
goal=head_goal,
result_cb=self.defaultHeadPositionComplete,
output_keys=['mission_status']),
transitions={
'succeeded': 'MOVE',
'preempted': 'preempted',
'aborted': 'aborted'
})
# This state uses an Action to move the robot to the required goal
StateMachine.add('MOVE',
SimpleActionState('move_base',
MoveBaseAction,
goal_slots=['target_pose'],
result_cb=self.moveComplete,
output_keys=['mission_status']),
transitions={
'succeeded': 'complete',
'preempted': 'preempted',
'aborted': 'aborted'
},
remapping={'target_pose': 'destination'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
arm = Designator(robot.leftArm)
with self:
StateMachine.add("FIND_CUP", CustomFindCup(robot),
transitions={'succeeded': 'GRAB',
'failed': 'failed'})
StateMachine.add("GRAB", SimpleGrab(robot, arm),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['user_answer'], output_keys=['text'])
with self:
# DONE integrate movements, speech and retrieval from akinator service
StateMachine.add('QUESTION',
AkinatorServiceState(),
transitions={'game_finished': 'aborted', 'continue_game': 'QUESTION_GESTURE'},
remapping={'resp_text': 'text', 'question_response': 'user_answer'}
)
AskingPool = ['CIR_Asking1', 'CIR_Asking2', 'CIR_Asking3', 'CIR_Asking4', 'CIR_Asking5', 'CIR_Asking6']
StateMachine.add('QUESTION_GESTURE',
SpeechGesture(wait_before_speak=1.5, behavior_pool=AskingPool),
transitions={'succeeded': 'succeeded'})
def __init__(self, robot, door_id):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
StateMachine.add("NAVIGATE_TO_DOOR",
NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=door_id)),
transitions={'arrived': 'OPEN_DOOR',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("OPEN_DOOR",
OpenDoor(robot, door_id),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, distance_threshold):
StateMachine.__init__(self, outcomes=['detected','failed','preempted'], input_keys=['sm_distance_threshold'], output_keys=['sm_pose'])
#self.userdata.sm_distance_threshold =
#self.userdata.required_still_checks =
with self:
smach.StateMachine.add('DETECT_CLOSE_PERSON', DetectClosePersonState(distance_threshold),
transitions = {'check_again':'DETECT_CLOSE_PERSON', 'done':'WAIT_FOR_STILL_PERSON', 'failed':'failed', 'preempted':'preempted'},
remapping = {'pose':'sm_pose'})
smach.StateMachine.add('WAIT_FOR_STILL_PERSON', WaitForStillPersonState(2),
transitions = {'check_again':'DETECT_CLOSE_PERSON','done':'detected','failed':'DETECT_CLOSE_PERSON','preempted':'preempted'},
remapping = {'pose':'sm_pose'})
def __init__(self, robot, cupboard_id, cupboard_navigation_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)
with self:
StateMachine.add("NAVIGATE_TO_CUPBOARD",
NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard),
transitions={'arrived': 'CLOSE_CUPBOARD',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("CLOSE_CUPBOARD", CloseCupboard(robot, cupboard_id),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, behavior_pool=None):
input_keys = []
if not behavior_pool:
input_keys = ['behavior_pool']
elif not isinstance(behavior_pool, list) and isinstance(behavior_pool, str):
behavior_pool = [behavior_pool]
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=input_keys)
for beh_name in behavior_pool:
_checkInstalledBehavior(beh_name)
with self:
StateMachine.add('SELECT_BEHAVIOR', RandomSelectionFromPoolState(behavior_pool),
transitions={'succeeded': 'EXECUTE_BEHAVIOR'}, remapping={'selected_item': 'behavior_name'})
StateMachine.add('EXECUTE_BEHAVIOR', ExecuteBehavior(), transitions={'succeeded': 'succeeded'})
def __init__(self,
testName='first',
dist_m_to_square=0.50,
go_to_square=True,
StartPose='Crouch'):
StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'])
with self:
StateMachine.add('HOME_ON',
HomeOn_SM(startPose=StartPose),
transitions={'succeeded': 'SAY_WAITING'})
text = '%s test start. To proceed please touch my head.' % testName
StateMachine.add('SAY_WAITING',
SpeechState(text=text, blocking=False),
transitions={'succeeded': 'WAIT_HEAD'})
StateMachine.add('WAIT_HEAD',
ReadTopicTactile(),
transitions={
'succeeded': 'STAND_INIT',
'aborted': 'SAY_NO_TOUCH'
})
text = 'Nobody touched my head in 30 seconds! Please touch my head.'
StateMachine.add('SAY_NO_TOUCH',
SpeechState(text=text, blocking=True),
transitions={'succeeded': 'WAIT_HEAD'})
StateMachine.add(
'STAND_INIT',
GoToPostureState('StandInit', 0.8),
transitions={
'succeeded':
'GO_TO_SQUARE' if go_to_square else 'succeeded'
})
text = "I'm going to the tag"
StateMachine.add('SAY_GOING_TO_TAG',
SpeechState(text=text, blocking=False),
transitions={'succeeded': 'WAIT_HEAD'})
StateMachine.add('GO_TO_SQUARE',
GoToSquare(min_x_dist=0.25,
dist_m_to_square=dist_m_to_square,
negative_vel=True),
transitions={'succeeded': 'succeeded'})
def __init__(self, robot, dishwasher_id, dishwasher_navigate_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id)
with self:
StateMachine.add("NAVIGATE_TO_DISHWASHER",
NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
transitions={'arrived': 'OPEN_DISHWASHER',
'unreachable': 'failed',
'goal_not_defined': 'failed'})
StateMachine.add("OPEN_DISHWASHER",
OpenDishwasher(robot, dishwasher_id),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
@cb_interface(outcomes=['succeeded'], output_keys=['position'])
def send_point(ud):
ud.position = PointStamped(header=Header(frame_id='/amigo/base_link'), point=Point(0.5, 0, 0.7))
return 'succeeded'
arm = Designator(robot.leftArm)
with self:
StateMachine.add("SEND_POINT", CBState(send_point),
transitions={'succeeded': 'FIND_CUP'})
StateMachine.add("FIND_CUP", SimpleGrab(robot, arm),
transitions={'succeeded': 'succeeded',
'failed': 'failed'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
arm = Designator(robot.leftArm)
with self:
StateMachine.add("FIND_CUP",
CustomFindCup(robot),
transitions={
'succeeded': 'GRAB',
'failed': 'failed'
})
StateMachine.add("GRAB",
SimpleGrab(robot, arm),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, min_y_step=-0.10, table_length=0.3): # 0.635 table length
StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
self._moved = 0
#############################################
#############################################
### Change begin orientation in next line ###
self._min_y_step = -min_y_step
#############################################
with self:
StateMachine.add('FIND_TOMATO', ReadTopicSquare(square_topic='/nao_tomato'), transitions={'succeeded': 'PREPARE_OBJ', 'aborted': 'PREPARE_LATERAL'},
remapping={'square': 'tomato'})
def put_lateral_obj(ud):
if self._moved >= table_length: # We have to go the other way around as we have overpassed the table
self._min_y_step = -self._min_y_step
self._moved = 0
# We move min_y_step meters or the what it's left to finish the table
y_mov = min(abs(self._min_y_step), table_length-self._moved)*math.copysign(1, self._min_y_step)
self._moved += abs(y_mov)
ud.objective = Pose2D(0.0, y_mov, 0.0)
return 'succeeded'
StateMachine.add('PREPARE_LATERAL', CBState(put_lateral_obj, output_keys=['objective'], outcomes=['succeeded']),
transitions={'succeeded':'LATERAL_MOVE'}, remapping={'objective': 'objective'})
StateMachine.add('LATERAL_MOVE', MoveToState(), transitions={'succeeded': 'FIND_TOMATO'})
def put_obj(ud):
transf_tomato = Pose2D(ud.tomato.y, ud.tomato.x, 0.0) #transform_pose(Pose2D(ud.tomato.x, ud.tomato.y, 0.0))
raw_input('***********' + str(transf_tomato) + ' ' + str(abs(transf_tomato.y) <= ALMOST_ZERO) )
if (abs(transf_tomato.y) <= ALMOST_ZERO):
return 'in_front'
obj = Pose2D(0.0, transf_tomato.y, 0.0)
ud.objective = obj
print '------------------ tomato_objective', obj
return 'succeeded'
StateMachine.add('PREPARE_OBJ', CBState(put_obj, outcomes=['succeeded', 'in_front'], input_keys=['tomato'], output_keys=['objective']),
transitions={'succeeded':'MOVE_TO_TOMATO', 'in_front': 'succeeded'}, remapping={'objective': 'objective'})
StateMachine.add('MOVE_TO_TOMATO', MoveToState(), transitions={'succeeded': 'succeeded'}, remapping={'objective': 'objective'})
def __init__(self, robot, rack_id, rack_navigation_area):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
rack = EdEntityDesignator(robot=robot, id=rack_id)
with self:
StateMachine.add("NAVIGATE_TO_RACK",
NavigateToSymbolic(robot,
{rack: rack_navigation_area},
rack),
transitions={
'arrived': 'GRAB_RACK',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
StateMachine.add("GRAB_RACK",
GrabRack(robot, rack_id),
transitions={'done': 'succeeded'})
def __init__(self, robot, arm):
StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
input_keys=['position'])
@cb_interface(outcomes=['succeeded'])
def prepare_grasp(ud):
# Open gripper (non-blocking)
arm.resolve().send_gripper_goal('open', timeout=0)
# Torso up (non-blocking)
robot.torso.high()
# Arm to position in a safe way
arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0)
# Open gripper
arm.resolve().send_gripper_goal('open', timeout=0.0)
return 'succeeded'
with self:
StateMachine.add(
"PREPARE_GRASP",
CBState(prepare_grasp),
transitions={'succeeded': 'SIMPLE_NAVIGATE_TO_GRASP'})
StateMachine.add("SIMPLE_NAVIGATE_TO_GRASP",
SimpleNavigateToGrasp(robot, arm),
transitions={
'unreachable': 'failed',
'arrived': 'SIMPLE_PICKUP',
'goal_not_defined': 'failed'
})
StateMachine.add("SIMPLE_PICKUP",
SimplePickup(robot, arm),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, get_one=True, ask_for_repetition=True, timeout=30):
''' If get_one is True, recognition_result is just the topic information. If it's false is a string with the most confident word.
If ask_for_repetition is True, the robot asks the user to repeat the response (for now it does it forever)
'''
StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], output_keys=['recognition_result'])
self.userdata.recognition_result = None
with self:
StateMachine.add('START_LISTEN',
StartRecognitionState(),
transitions={'succeeded': 'ANSWER_DETECTION'})
StateMachine.add('ANSWER_DETECTION',
GetRecognizedWordNoEmptyList(timeout=timeout),
transitions={'succeeded': 'STOP_LISTEN',
'timeouted': 'STOP_LISTEN_ABORTED',
'preempted': 'preempted'},
remapping={'recognized_words': 'recognition_result'})
StateMachine.add('STOP_LISTEN',
StopRecognitionState(),
transitions={'succeeded': 'GET_SINGLE_RESPONSE' if get_one else 'succeeded'})
def get_most_confident_word(ud):
sorted_result = sorted(zip(ud.in_recognition_result.confidence_values, ud.in_recognition_result.words), reverse=True)
ud.out_recognition_result = sorted_result[0][1] # Get the most confident [0] word [1]
rospy.loginfo('---Get user answer recognized word: ' + sorted_result[0][1])
return 'succeeded'
StateMachine.add('GET_SINGLE_RESPONSE',
CBState(get_most_confident_word, input_keys=['in_recognition_result'], output_keys=['out_recognition_result'], outcomes=['succeeded']),
remapping={'in_recognition_result': 'recognition_result', 'out_recognition_result': 'recognition_result'},
transitions={'succeeded': 'succeeded'})
StateMachine.add('STOP_LISTEN_ABORTED',
StopRecognitionState(),
transitions={'succeeded': 'aborted' if not ask_for_repetition else 'ASK_TO_REPEAT'})
repeat_pool = ['I could not hear you. Repeat it please.', 'Did you say anything? Please repeat it.', 'I did not understand anything. Answer my question.',
'Please, answer my question.']
StateMachine.add('ASK_TO_REPEAT', SpeechFromPoolSM(pool=repeat_pool, blocking=True), transitions={'succeeded': 'START_LISTEN'})
def __init__(self, timeout=10):
sm.__init__(
self,
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'],
input_keys=['gender_recognition_goal'],
output_keys=[
'gender_recognition_feedback', 'gender_recognition_result'
])
gender_model_path = rospy.get_param('~gender_model_path', '')
image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw')
labels = {0: 'woman', 1: 'man'}
image_size = (64, 64, 1)
with self:
sm.add('SETUP_GENDER_RECOGNITION',
SetupGenderRecognition(),
transitions={
'succeeded': 'RECOGNIZE_GENDERS',
'failed': 'SETUP_GENDER_RECOGNITION'
})
sm.add('RECOGNIZE_GENDERS',
RecognizeGenders(gender_model_path=gender_model_path,
image_topic=image_topic,
labels=labels,
image_size=image_size),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'SET_ACTION_LIB_FAILED'
})
sm.add('SET_ACTION_LIB_FAILED',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
sm.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
def __init__(self):
StateMachine.__init__(self, outcomes=[succeeded, aborted, preempted, 'move_failed', 'no_object_found'],
input_keys=['in_location_pose_in_map', 'in_target_object'],
output_keys=['out_objects_data'])
self.userdata.out_objects_data = None # If the move action fails, this output key is not filled and shows an error.
# SM that moves to searching location and searches objects
with self:
StateMachine.add('MOVE_TO_SEARCHING_LOCATION',
MoveActionState("/map", goal_key='in_location_pose_in_map'),
transitions={succeeded: 'RECOGNIZE_OBJECTS', aborted: 'move_failed'})
StateMachine.add('RECOGNIZE_OBJECTS', SearchObjectWithConfidenceStateMachine(),
remapping={'object_found': 'out_objects_data',
'object_to_search_for': 'in_target_object'},
transitions={succeeded: succeeded, aborted: 'RETRY_RECOGNIZE_OBJECTS'})
StateMachine.add('RETRY_RECOGNIZE_OBJECTS', SearchObjectWithConfidenceStateMachine(),
remapping={'object_found': 'out_objects_data',
'object_to_search_for': 'in_target_object'},
transitions={succeeded: succeeded, aborted: 'no_object_found'})
def __init__(self, robot, source_entity_designator, area_name_designator, navigation_area_designator,
found_entity_designator):
StateMachine.__init__(self, outcomes=['found', 'not_found'])
segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult])
with self:
StateMachine.add('SAY_LOOKING', Say(robot, ["I'm going to look for the cup"], block=False),
transitions={'spoken': 'INSPECT_SOURCE_ENTITY'})
StateMachine.add('INSPECT_SOURCE_ENTITY', Inspect(robot=robot,
entityDes=source_entity_designator,
objectIDsDes=segmented_entities_designator,
searchArea=area_name_designator,
navigation_area=navigation_area_designator),
transitions={'done': 'GET_CUP_ID',
'failed': 'not_found'})
StateMachine.add('GET_CUP_ID', GetCupId(robot=robot,
found_entity_designator=found_entity_designator.writeable,
candidate_entities_designator=segmented_entities_designator),
transitions={'succeeded': 'found',
'failed': 'not_found'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=["succeeded", "failed"])
@cb_interface(outcomes=['succeeded'], output_keys=['position'])
def send_point(ud):
ud.position = PointStamped(
header=Header(frame_id='/amigo/base_link'),
point=Point(0.5, 0, 0.7))
return 'succeeded'
arm = Designator(robot.leftArm)
with self:
StateMachine.add("SEND_POINT",
CBState(send_point),
transitions={'succeeded': 'FIND_CUP'})
StateMachine.add("FIND_CUP",
SimpleGrab(robot, arm),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, dist_m_to_square=0.50, min_x_dist=0.25, negative_vel=False):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])
self.ALMOST_ZERO = 0.005
self.negative_vel = negative_vel
with self:
StateMachine.add('FIND_SQUARE', FindSquare(), transitions={'succeeded': 'PREPARE_OBJ'}, remapping={'square': 'square'})
def put_obj(ud):
transf_square = transform_pose(Pose2D(ud.square.z, ud.square.x, 0.0))
if not self.negative_vel:
y_mov = transf_square.y
else:
y_mov = -1*abs(transf_square.y)
x_mov = min(min_x_dist, abs(transf_square.x)-dist_m_to_square)
print '------------------ ud.square', ud.square
print '------------------ transf_square', transf_square
print '------------------ x_mov', x_mov
print '------------------ min_x_dist', min_x_dist
print '------------------ dist_m_to_square', dist_m_to_square
#if x_mov <= self.ALMOST_ZERO and ud.square.x <= self.ALMOST_ZERO:
obj = Pose2D(x_mov, y_mov, 0.0)
ud.objective = obj
print '------------------ objective', obj
if x_mov < min_x_dist:
return 'one_step_left'
else:
return 'succeeded'
StateMachine.add('PREPARE_OBJ', CBState(put_obj, outcomes=['succeeded', 'one_step_left'], input_keys=['square'], output_keys=['objective']),
transitions={'succeeded': 'MOVE_TO_SQ', 'one_step_left': 'MOVE_TO_FINAL'}, remapping={'objective': 'objective'})
StateMachine.add('MOVE_TO_SQ', MoveToState(), transitions={'succeeded': 'FIND_SQUARE'}, remapping={'objective': 'objective'})
StateMachine.add('MOVE_TO_FINAL', MoveToState(), transitions={'succeeded': 'succeeded'}, remapping={'objective': 'objective'})
def __init__(self, robot, cupboard_id, required_items):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'], output_keys=["item_picked"])
arm = robot.get_arm()._arm
picked_items = []
def send_joint_goal(position_array, wait_for_motion_done=True):
arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0))
if wait_for_motion_done:
arm.wait_for_motion_done()
def send_gripper_goal(open_close_string, max_torque=0.1):
arm.send_gripper_goal(open_close_string, max_torque=max_torque)
rospy.sleep(1.0) # Does not work with motion_done apparently
def show_image(package_name, path_to_image_in_package):
path = os.path.join(rospkg.RosPack().get_path(package_name), path_to_image_in_package)
if not os.path.exists(path):
rospy.logerr("Image path {} does not exist".format(path))
else:
try:
rospy.loginfo("Showing {}".format(path))
robot.hmi.show_image(path, 10)
except Exception as e:
rospy.logerr("Could not show image {}: {}".format(path, e))
return 'succeeded'
@cb_interface(outcomes=['succeeded', 'failed'], output_keys=["item_picked"])
def _ask_user(user_data):
leftover_items = [item for item in required_items if item not in picked_items]
if not leftover_items:
robot.speech.speak("We picked 'm all apparently")
return 'failed'
item_name = leftover_items[0]
if item_name == 'plate':
send_joint_goal(plate_handover)
else:
arm.send_joint_goal("carrying_pose")
picked_items.append(item_name)
robot.speech.speak("Please put the {} in my gripper, like this".format(item_name), block=False)
show_image('challenge_set_the_table', item_img_dict[item_name])
send_gripper_goal("open")
rospy.sleep(5.0)
robot.speech.speak("Thanks for that!", block=False)
if item_name == 'plate':
send_gripper_goal("close", max_torque=0.7)
else:
send_gripper_goal("close")
# Set output data
user_data['item_picked'] = item_name
arm.send_joint_goal("carrying_pose")
return 'succeeded'
with self:
self.add('ASK_USER', CBState(_ask_user), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def __init__(self, robot, room_id):
StateMachine.__init__(self, outcomes=['done'])
@cb_interface(outcomes=['done'])
def detect_persons(_):
global PERSON_DETECTIONS
global NUM_LOOKS
# with open('/home/rein/mates/floorplan-2019-07-05-11-06-52.pickle', 'r') as f:
# PERSON_DETECTIONS = pickle.load(f)
# rospy.loginfo("Loaded %d persons", len(PERSON_DETECTIONS))
#
#
# return "done"
look_angles = np.linspace(-np.pi / 2, np.pi / 2, 8) # From -pi/2 to +pi/2 to scan 180 degrees wide
head_goals = [kdl_conversions.VectorStamped(x=100 * math.cos(angle),
y=100 * math.sin(angle),
z=1.5,
frame_id="/%s/base_link" % robot.robot_name)
for angle in look_angles]
sentences = deque([
"Hi there mates, where are you, please look at me!",
"I am looking for my mates! Dippi dee doo! Pew pew!",
"You are all looking great today! Keep looking at my camera. I like it when everybody is staring at me!"
])
while len(PERSON_DETECTIONS) < 4 and not rospy.is_shutdown():
for _ in range(NUM_LOOKS):
sentences.rotate(1)
robot.speech.speak(sentences[0], block=False)
for head_goal in head_goals:
robot.speech.speak("please look at me", block=False)
robot.head.look_at_point(head_goal)
robot.head.wait_for_motion_done()
now = time.time()
rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo()
try:
persons = robot.perception.detect_person_3d(rgb, depth, depth_info)
except Exception as e:
rospy.logerr(e)
rospy.sleep(2.0)
else:
for person in persons:
if person.face.roi.width > 0 and person.face.roi.height > 0:
try:
PERSON_DETECTIONS.append({
"map_ps": robot.tf_listener.transformPoint("map", PointStamped(
header=rgb.header,
point=person.position
)),
"person_detection": person,
"rgb": rgb
})
except Exception as e:
rospy.logerr("Failed to transform valid person detection to map frame")
rospy.loginfo("Took %.2f, we have %d person detections now", time.time() - now, len(PERSON_DETECTIONS))
rospy.loginfo("Detected %d persons", len(PERSON_DETECTIONS))
return 'done'
@cb_interface(outcomes=['done', 'failed'])
def _data_association_persons_and_show_image_on_screen(_):
global PERSON_DETECTIONS
try:
with open(os.path.expanduser('~/floorplan-{}.pickle'.format(datetime.now().strftime("%Y-%m-%d-%H-%M-%S"))), 'w') as f:
pickle.dump(PERSON_DETECTIONS, f)
except:
pass
room_entity = robot.ed.get_entity(id=room_id) # type: Entity
room_volume = room_entity.volumes["in"]
min_corner = room_entity.pose.frame * room_volume.min_corner
max_corner = room_entity.pose.frame * room_volume.max_corner
shrink_x = 0.5
shrink_y = 0.3
min_corner_shrinked = PyKDL.Vector(min_corner.x() + shrink_x, min_corner.y() + shrink_y, 0)
max_corner_shrinked = PyKDL.Vector(max_corner.x() - shrink_x, max_corner.y() - shrink_y, 0)
rospy.loginfo('Found %d person detections', len(PERSON_DETECTIONS))
def _get_clusters():
def _in_room(p):
return min_corner_shrinked.x() < p.x < max_corner_shrinked.x() and min_corner_shrinked.y() < p.y < max_corner_shrinked.y()
in_room_detections = [d for d in PERSON_DETECTIONS if _in_room(d['map_ps'].point)]
rospy.loginfo("%d in room before clustering", len(in_room_detections))
clusters = cluster_people(in_room_detections, np.array([6, 0]))
return clusters
# filter in room and perform clustering until we have 4 options
try:
person_detection_clusters = _get_clusters()
except ValueError as e:
rospy.logerr(e)
robot.speech.speak("Mates, where are you?", block=False)
return "failed"
floorplan = cv2.imread(
os.path.join(rospkg.RosPack().get_path('challenge_find_my_mates'), 'img/floorplan.png'))
floorplan_height, floorplan_width, _ = floorplan.shape
bridge = CvBridge()
c_map = color_map(N=len(person_detection_clusters), normalized=True)
for i, person_detection in enumerate(person_detection_clusters):
image = bridge.imgmsg_to_cv2(person_detection['rgb'], "bgr8")
roi = person_detection['person_detection'].face.roi
roi_image = image[roi.y_offset:roi.y_offset + roi.height, roi.x_offset:roi.x_offset + roi.width]
desired_height = 150
height, width, channel = roi_image.shape
ratio = float(height) / float(desired_height)
calculated_width = int(float(width) / ratio)
resized_roi_image = cv2.resize(roi_image, (calculated_width, desired_height))
x = person_detection['map_ps'].point.x
y = person_detection['map_ps'].point.y
x_image_frame = 9.04 - x
y_image_frame = 1.58 + y
pixels_per_meter = 158
px = int(pixels_per_meter * x_image_frame)
py = int(pixels_per_meter * y_image_frame)
cv2.circle(floorplan, (px, py), 3, (0,0,255), 5)
try:
px_image = min(max(0, px - calculated_width / 2), floorplan_width - calculated_width - 1)
py_image = min(max(0, py - desired_height / 2), floorplan_height - desired_height - 1)
if px_image >= 0 and py_image >= 0:
#could not broadcast input array from shape (150,150,3) into shape (106,150,3)
floorplan[py_image:py_image + desired_height, px_image:px_image + calculated_width] = resized_roi_image
cv2.rectangle(floorplan, (px_image, py_image),
(px_image + calculated_width, py_image + desired_height),
(c_map[i, 2] * 255, c_map[i, 1] * 255, c_map[i, 0] * 255), 10)
else:
rospy.logerr("bound error")
except Exception as e:
rospy.logerr("Drawing image roi failed: {}".format(e))
label = "female" if person_detection['person_detection'].gender else "male"
label += ", " + str(person_detection['person_detection'].age)
cv2.putText(floorplan, label, (px_image, py_image + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA)
# cv2.circle(floorplan, (px, py), 3, (0, 0, 255), 5)
filename = os.path.expanduser('~/floorplan-{}.png'.format(datetime.now().strftime("%Y-%m-%d-%H-%M-%S")))
cv2.imwrite(filename, floorplan)
robot.hmi.show_image(filename, 120)
return "done"
with self:
self.add_auto('DETECT_PERSONS', CBState(detect_persons), ['done'])
self.add('DATA_ASSOCIATION_AND_SHOW_IMAGE_ON_SCREEN',
CBState(_data_association_persons_and_show_image_on_screen), transitions={'done': 'done', 'failed': 'DETECT_PERSONS'})
def __init__(self, robot, dishwasher_id):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
base_y_position_door = 0
base_y_position_rack = -0.75
base_y_position_rack2 = -0.65
base_yaw_position_rack = 3.7
@cb_interface(outcomes=['done'])
def _pre_grab_handle(ud):
robot.rightArm.send_gripper_goal("open", timeout=0)
robot.rightArm._send_joint_trajectory([[0, 0.2519052373022729913, 0.7746500794619434, 1.3944848321343395,
-1.829999276180074, 0.6947045024700284, 0.1889253710114966]],
timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
return 'done'
@cb_interface(outcomes=['done'])
def _grab_handle(ud):
robot.rightArm.wait_for_motion_done()
robot.speech.speak('I hope this goes right!', block=False)
fs = frame_stamped("dishwasher", 0.42, 0, 0.8, roll=math.pi / 2, pitch=0, yaw=math.pi)
robot.rightArm.send_goal(fs.projectToFrame(robot.robot_name + "/base_link", robot.tf_listener))
robot.rightArm.send_gripper_goal("close")
return 'done'
@cb_interface(outcomes=['done'])
def _align_with_dishwasher(ud):
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = dishwasher_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
goal_pose.pose.position.x = 0.85
goal_pose.pose.position.y = base_y_position_door
ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01, 0.1)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def _drive_to_open_dishwasher(ud):
# Open the dishwasher
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = dishwasher_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
goal_pose.pose.position.x = 1.4
goal_pose.pose.position.y = base_y_position_door
robot.torso.low()
ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.1, 0.05, 0.5)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def _fully_open_dishwasher_door(ud):
# Stand in front of the door and rotate
robot.torso.high()
robot.rightArm._send_joint_trajectory([[0, 0, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0))
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = dishwasher_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, - math.pi / 2))
goal_pose.pose.position.x = 1.4
goal_pose.pose.position.y = base_y_position_door
ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.5, 0.5, 0.5, 0.05, 0.1)).execute({})
robot.torso.wait_for_motion_done()
robot.rightArm.wait_for_motion_done()
# Move arm to the other side of the door
robot.rightArm._send_joint_trajectory([[0, 1.57, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
robot.rightArm._send_joint_trajectory([[-1.5, 1.57, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
robot.rightArm._send_joint_trajectory([[-1.5, 0, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
# Go down
robot.torso._send_goal([0.1])
robot.torso.wait_for_motion_done()
robot.rightArm._send_joint_trajectory([[-0.8, 0, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
# Drive away from the door so we open the door with our stretched arm
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = dishwasher_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, - math.pi / 2))
goal_pose.pose.position.x = 1.9
goal_pose.pose.position.y = base_y_position_door
ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.1, 0.1, 0.1, 0.05, 0.1)).execute({})
robot.rightArm.reset()
robot.rightArm.wait_for_motion_done()
return 'done'
@cb_interface(outcomes=['done'])
def _align_with_dishwasher_to_open_rack(ud):
robot.torso.low()
# Drive sideways around the open door
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = dishwasher_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi))
goal_pose.pose.position.x = 1.4
goal_pose.pose.position.y = base_y_position_rack - 0.15
ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.1, 0.1, 0.1, 0.02, 0.2)).execute({})
# Arm in the right position so we can drive in the dishwasher with the arm
robot.rightArm._send_joint_trajectory([[-1.48, 0, 0, 0.5, 1.57, 0, -0.1]], timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
robot.torso.wait_for_motion_done()
# Drive aside the open door, with arm in the dishwasher
goal_pose.pose.position.x = 0.8
goal_pose.pose.position.y = base_y_position_rack2
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, base_yaw_position_rack))
ControlToPose(robot, goal_pose, ControlParameters(0.5, 0.5, 0.1, 0.1, 0.1, 0.02, 0.2)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def _arm_in_rack(ud):
robot.rightArm._send_joint_trajectory([[-1.4, 0, 0, 0.45, 1.57, 0.8, -0.1]], timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
return 'done'
@cb_interface(outcomes=['done'])
def _drive_to_open_rack(ud):
# Open the dishwasher rack
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = dishwasher_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, base_yaw_position_rack))
goal_pose.pose.position.x = 1.25
goal_pose.pose.position.y = base_y_position_rack2
ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.1, 0.05, 0.2)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def _retract_arm_from_rack(ud):
robot.torso.reset()
robot.torso.wait_for_motion_done()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = dishwasher_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0.9 * math.pi))
goal_pose.pose.position.x = 1.25
goal_pose.pose.position.y = base_y_position_rack2
ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.4, 0.1, 0.2)).execute({})
robot.rightArm.reset()
robot.rightArm.wait_for_motion_done()
return 'done'
with self:
self.add_auto('PRE_GRAB_HANDLE', CBState(_pre_grab_handle), ['done'])
self.add_auto('ALIGN_WITH_DISHWASHER', CBState(_align_with_dishwasher), ['done'])
self.add_auto('GRAB_HANDLE', CBState(_grab_handle), ['done'])
self.add_auto('DRIVE_TO_OPEN_DISHWASHER', CBState(_drive_to_open_dishwasher), ['done'])
self.add_auto('FULLY_OPEN_DISHWASHER_DOOR', CBState(_fully_open_dishwasher_door), ['done'])
self.add_auto('ALIGN_WITH_DISHWASHER_TO_OPEN_RACK', CBState(_align_with_dishwasher_to_open_rack), ['done'])
self.add_auto('ARM_IN_RACK', CBState(_arm_in_rack), ['done'])
self.add_auto('DRIVE_TO_OPEN_RACK', CBState(_drive_to_open_rack), ['done'])
self.add('RETRACT_ARM_FROM_RACK', CBState(_retract_arm_from_rack), transitions={'done': 'succeeded'})
def __init__(self):
StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])
self.turn_rad = REORIENT_RAD
with self:
StateMachine.add('MOVE_LEFT', MoveToState(Pose2D(0.0, LEFT_MOVE, 0.0)), transitions={'succeeded': 'MOVE_TO_FRONT'})
StateMachine.add('MOVE_TO_FRONT', MoveToState(Pose2D(FRONT_MOVE, 0.0, 0.0)), transitions={'succeeded': 'SAY_CHECKING_FIRES'})
text = 'I will check if any fire is lit.'
StateMachine.add('SAY_CHECKING_FIRES', SpeechState(text=text, blocking=False), transitions={'succeeded': 'LOOK_AT_STOVE'})
StateMachine.add('LOOK_AT_STOVE', JointAngleState(['HeadPitch', 'RElbowRoll', 'LElbowRoll'], [0.35, 0.05, -0.05]),
transitions={'succeeded': 'CHECK_FIRE'})
#StateMachine.add('DISABLE_ARM_WALK', SetArmsWalkingState(leftArmEnabled=False, rightArmEnabled=False),
# transitions={'succeeded': 'LATERAL_TO_FIRE'})
#StateMachine.add('LATERAL_TO_FIRE', MoveToState(Pose2D(0.0, DISTANCE_MARKER_TO_FIRE, 0.0)), transitions={'succeeded': 'CHECK_FIRE'})
StateMachine.add('CHECK_FIRE', ReadTopicFire(),
transitions={'succeeded': 'PREPARE_TEXT_AND_MOVEMENT', 'aborted': 'REORIENT_FIRE'}, remapping={'plate': 'plate'})
StateMachine.add('REORIENT_FIRE', MoveToState(Pose2D(0.0, 0.0, REORIENT_FIRE)), transitions={'succeeded': 'CHECK_FIRE'})
def prep_text(ud):
ud.out_text = "The %s fire is lit! I will put it off." % ud.plate.lower()
if ud.plate == "UPPER":
ud.objective = Pose2D(0.0, -DISTANCE_FIRE_UPPER_BUTTON, 0.0)
else:
ud.objective = Pose2D(0.0, -DISTANCE_FIRE_LOWER_BUTTON, 0.0)
return 'succeeded'
StateMachine.add('PREPARE_TEXT_AND_MOVEMENT', CBState(prep_text, outcomes=['succeeded'],
input_keys=['plate'], output_keys=['out_text', 'objective']),
transitions={'succeeded':'SAY_FIRE_LIT'}, remapping={'out_text':'text', 'objective': 'objective'})
StateMachine.add('SAY_FIRE_LIT', SpeechState(blocking=False), remapping={'text': 'text'},
transitions={'succeeded': 'PREPARE_EXECUTE'})
def prep_execute_putoff(ud):
if ud.plate == "UPPER":
return 'upper'
else:
return 'lower'
return 'succeeded'
StateMachine.add('PREPARE_EXECUTE', CBState(prep_execute_putoff, outcomes=['upper', 'lower', 'succeeded'],
input_keys=['plate']),
transitions={'succeeded':'SAY_NO_FIRE_LIT', 'lower': 'PUT_OFF_LOWER_FIRE_MOVEMENT', 'upper': 'PUT_OFF_UPPER_FIRE_MOVEMENT'},
#transitions={'lower': 'succeeded', 'upper':'succeeded'},
remapping={'out_text':'text', 'objective': 'objective'})
#StateMachine.add('MOVE_TO_BUTTON', MoveToState(), transitions={'succeeded': 'PUT_OFF_FIRE_MOVEMENT'},
# remapping={'objective': 'objective'})
StateMachine.add('PUT_OFF_UPPER_FIRE_MOVEMENT', ExecuteBehavior(behavior_name='putoff_upper_fire'), transitions={'succeeded':'RE_CHECK'})
StateMachine.add('PUT_OFF_LOWER_FIRE_MOVEMENT', ExecuteBehavior(behavior_name='putoff_lower_fire'), transitions={'succeeded':'RE_CHECK'})
StateMachine.add('RE_CHECK', ReadTopicFire(),
transitions={'succeeded': 'SAY_FAILED', 'aborted': 'SAY_FINISH'}, remapping={'plate': 'plate'})
StateMachine.add('SAY_FAILED', SpeechState(text='Oh I failed!', blocking=False), transitions={'succeeded': 'PREPARE_REORIENT'})
def prep_reorientcheck(ud):
ud.objective = Pose2D(0.0, 0.0, self.turn_rad)
self.turn_rad = -self.turn_rad
return 'succeeded'
StateMachine.add('PREPARE_REORIENT', CBState(prep_reorientcheck, outcomes=['succeeded'], output_keys=['objective']),
transitions={'succeeded': 'REORIENT'})
StateMachine.add('REORIENT', MoveToState(), transitions={'succeeded': 'PUT_OFF_UPPER_FIRE_MOVEMENT'})
StateMachine.add('REPUTOFF', ExecuteBehavior(behavior_name='putoff_upper_fire'), transitions={'succeeded':'LAST_CHECK'})
StateMachine.add('LAST_CHECK', ReadTopicFire(),
transitions={'succeeded': 'SAY_EVACUATE', 'aborted': 'SAY_FINISH'}, remapping={'plate': 'plate'})
StateMachine.add('SAY_EVACUATE', SpeechState(text='I could not put off the fire. Alarm! Please evacuate the room.', blocking=False),
transitions={'succeeded': 'GO_BACK'})
StateMachine.add('SAY_NO_FIRE_LIT', SpeechState(text='I am done as no fire is lit.', blocking=False), transitions={'succeeded': 'GO_BACK'})
text = 'I am done, there is no need to call the fire department!'
StateMachine.add('SAY_FINISH', SpeechState(text=text, blocking=False), transitions={'succeeded': 'GO_BACK'})
StateMachine.add('GO_BACK', MoveToState(Pose2D(-DISTANCE_BACK_FROM_FIRE, 0.0, 0.0)), transitions={'succeeded': 'succeeded'})
def __init__(self, num_persons, all_at_a_time, room_name, sleep, ask_to_come):
""" Constructor for TakeServeDrinkOrdersSM
:type num_persons: integer
:param num_persons: The number of persons to be served
:type all_at_a_time: boolean
:param all_at_a_time: If True, the robot will take all drink orders first,\
and after this, will serve, otherwise will take and serve ONE drink at a time.
:type room_name: string
:param room_name: The room where the robot should go to take drink orders \
if 'all_at_a_time' is not True
:type sleep: int
:param sleep: The number of seconds that the robot will sleep after ask\
the person to come if MoveToCaller aborts
:type ask_to_come: bool
:param ask_to_come: If true, the robot will ask the person to come and\
wait $sleep seconds. Otherwise, will move to caller.
"""
assert type(all_at_a_time) is bool
assert type(num_persons) is int and num_persons > 0
assert type(room_name) is str and len(room_name) > 0
assert type(sleep) is int and sleep > 0
StateMachine.__init__(self, input_keys=[], output_keys=[], outcomes=[succeeded, aborted, preempted])
NUMBER_PERSONS = 1
TAKE_ORDER_STATE= "TAKE_DRINK_ORDER"
DEBUG_STATE = "DEBUG_DRINK_ORDER"
SERVE_ORDER_STATE = "SERVE_ORDER"
FINAL_STATE = "CHECK_REPEAT"
MOVE_BACK_ROOM = "MOVE_BACK_PARTY_ROOM"
with self:
if all_at_a_time is True:
NUMBER_PERSONS = num_persons
TAKE_ORDER_STATE += "S"
DEBUG_STATE += "S"
SERVE_ORDER_STATE += "S"
FINAL_STATE = succeeded
def start_cb(userdata, initial_st_name):
""" Set the variable userdata.room_name used by MoveToRoom """
userdata.room_name = room_name
self.register_start_cb(start_cb)
StateMachine.add(
TAKE_ORDER_STATE,
TakeSeveralDrinkOrdersStateMachine(num_persons=NUMBER_PERSONS, sleep=sleep, ask_to_come=ask_to_come),
transitions={succeeded: DEBUG_STATE}
#The aborted transition is not been catched because in theory will never abort.
)# outputs: "drink_orders"
def print_drink_orders(userdata):
print C.WHITE_BOLD + "DRINK ORDER(S)"
for order in userdata.drink_orders:
loginfo( "Person: %-10s, Drink: %s" % (order.person_name, order.drink))
print C.NATIVE_COLOR
return succeeded
StateMachine.add(
DEBUG_STATE,
CBState(print_drink_orders, input_keys=["drink_orders"], outcomes=[succeeded, aborted]),
transitions={succeeded: SERVE_ORDER_STATE, aborted: SERVE_ORDER_STATE}
)
StateMachine.add(
SERVE_ORDER_STATE,
ServeOrdersStateMachine(number_persons=NUMBER_PERSONS),
transitions={succeeded: FINAL_STATE, aborted: FINAL_STATE, preempted: FINAL_STATE}
)
if all_at_a_time is not True:
self.people_served = 0
def check_repeat(userdata):
""" Check if still should take and serve drinks"""
self.people_served += 1
if self.people_served < num_persons:
return succeeded
return aborted
StateMachine.add(
FINAL_STATE,
CBState(check_repeat, outcomes=[succeeded,aborted]),
transitions={succeeded: MOVE_BACK_ROOM, aborted: succeeded}
)
StateMachine.add(
MOVE_BACK_ROOM,
MoveToRoomStateMachine(announcement="I'm going to %s to take another drink order."),
transitions={succeeded: TAKE_ORDER_STATE, aborted: TAKE_ORDER_STATE, preempted: TAKE_ORDER_STATE}
)
def __init__(self, robot, door_id):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
@cb_interface(outcomes=['done'])
def _pre_grab_handle(ud):
robot.rightArm.send_gripper_goal("open", timeout=0)
robot.rightArm._send_joint_trajectory([[0,0.7,0,1.3,1.57,0,0.4]], timeout=rospy.Duration(0))
robot.rightArm.wait_for_motion_done()
return 'done'
@cb_interface(outcomes=['done'])
def _align_with_door(ud):
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = door_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0))
goal_pose.pose.position.x = 0.311
goal_pose.pose.position.y = -0.013
ControlToPose(robot, goal_pose, ControlParameters(0.5, 0.5, 0.1, 0.1, 0.1, 0.01, 0.1)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def _grab_handle(ud):
robot.rightArm.send_gripper_goal("close")
robot.rightArm.wait_for_motion_done()
return 'done'
@cb_interface(outcomes=['done'])
def _drive_to_open_door(ud):
# Open the door 1
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = door_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, -0.609))
goal_pose.pose.position.x = -0.228
goal_pose.pose.position.y = 0.9
ControlToPose(robot, goal_pose, ControlParameters(0.5, 0.5, 0.1, 0.1, 0.1, 0.01, 0.1)).execute({})
# Open the door 2
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = door_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0))
goal_pose.pose.position.x = -0.350
goal_pose.pose.position.y = 0.9
ControlToPose(robot, goal_pose, ControlParameters(0.5, 0.5, 0.1, 0.1, 0.1, 0.01, 0.1)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def _retract_arm(ud):
robot.rightArm.send_gripper_goal("open")
robot.rightArm.wait_for_motion_done()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = door_id
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0))
goal_pose.pose.position.x = -0.803
goal_pose.pose.position.y = 0.986
ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.4, 0.1, 0.2)).execute({})
robot.rightArm.send_gripper_goal("close")
robot.torso.reset()
robot.rightArm.wait_for_motion_done()
robot.torso.wait_for_motion_done()
return 'done'
with self:
self.add_auto('PRE_GRAB_HANDLE', CBState(_pre_grab_handle), ['done'])
self.add_auto('ALIGN_WITH_DOOR', CBState(_align_with_door), ['done'])
self.add_auto('GRAB_HANDLE', CBState(_grab_handle), ['done'])
self.add_auto('DRIVE_TO_OPEN_DOOR', CBState(_drive_to_open_door), ['done'])
self.add('RETRACT_ARM', CBState(_retract_arm), transitions={'done': 'succeeded'})
def __init__(self, driver=None):
StateMachine.__init__(self, outcomes=['succeeded',
'preempted', 'aborted'])
self.userdata.text = ''
with self:
StateMachine.add('PREPARE',
HomeOn_SM('Sit'),
transitions={'succeeded': 'INIT'})
#transitions={'succeeded': 'GAME'}) #DEBUGGING LINE
#PREPARE NAO
setup = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with setup:
StateMachine.add('CLEAN_LISTEN_STATE',
StopRecognitionState(),
transitions={'succeeded': 'SET_VOCABULARY'}
)
StateMachine.add('SET_VOCABULARY',
SetSpeechVocabularyState(no_words + yes_words + probably_not_words + dunno_words + maybe_words),
transitions={'succeeded': 'RESET_AKINATOR'}
)
#RESET AKINATOR WEB
StateMachine.add('RESET_AKINATOR',
ServiceState('/reset_akinator_params',
ResetAkinator,
request=ResetAkinatorRequest('Name', 22)),
transitions={'succeeded': 'succeeded'}
)
cc = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='aborted',
outcome_map={'succeeded': {'START_GAME_INTRO': 'succeeded', 'SETUP': 'succeeded'}})
with cc:
PresentationPool = ['CIR_Presentation1', 'CIR_Presentation2']
# INTRODUCTION OF THE GAME
Concurrence.add('START_GAME_INTRO',
ExecuteBehaviorFromPoolSM(behavior_pool=PresentationPool))
# NAO SETUP
Concurrence.add('SETUP', setup)
StateMachine.add('INIT', cc,
transitions={'succeeded': 'GAME', 'aborted': 'LOSE'})
#GAME LOOP
StateMachine.add('GAME',
AkinatorGame(),
transitions={'succeeded': 'CHAR_QUESTION_MAKE', 'aborted': 'LOSE'}
)
#END OF GAME
StateMachine.add('CHAR_QUESTION_MAKE',
CBState(self.compoundQuestion,
input_keys=['text'],
output_keys=['text'],
outcomes=['succeeded', 'aborted']),
transitions={'succeeded': 'CHAR_CORRECT'}
)
StateMachine.add('CHAR_CORRECT',
SpeechGesture(behavior_pool=['CIR_Asking1', 'CIR_Asking2', 'CIR_Asking3', 'CIR_Asking4', 'CIR_Asking5', 'CIR_Asking6']),
transitions={'succeeded': 'FINAL_ANSWER'}
)
StateMachine.add('FINAL_ANSWER',
SpeechRecognitionAndGesture(),
transitions={'succeeded': 'CHECK_WIN_LOSE', 'aborted': 'LOSE'}
)
def check_w_l(userdata):
if (userdata.text == 'yes'):
return 'win'
return 'lose'
StateMachine.add('CHECK_WIN_LOSE',
CBState(check_w_l, input_keys=['text'], outcomes=['win', 'lose']),
transitions={'win': 'WIN', 'lose': 'LOSE'})
StateMachine.add('WIN',
ExecuteBehaviorFromPoolSM(behavior_pool=['CIR_Winning1', 'CIR_Winning2', 'CIR_Winning3', 'CIR_Winning4', 'CIR_Winning5']),
transitions={'succeeded': 'DISABLE_STIFF'})
StateMachine.add('LOSE',
#ExecuteBehavior(behavior_name='CIR_Losing1'),
ExecuteBehaviorFromPoolSM(behavior_pool=['CIR_Loosing1', 'CIR_Loosing2', 'CIR_Loosing3', 'CIR_Loosing4']),
transitions={'succeeded': 'DISABLE_STIFF'})
StateMachine.add('DISABLE_STIFF',
DisableStiffnessState(),
transitions={'succeeded': 'succeeded'})
def __init__(self, robot):
StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
arm = Designator(robot.leftArm)
@cb_interface(outcomes=['done'])
def pre_place_pose(ud):
robot.torso.high()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'map'
goal_pose.pose.position.x = -4.55638664735
goal_pose.pose.position.y = 4.99959353359
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0.395625992))
ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01, 0.1)).execute({})
robot.torso.wait_for_motion_done()
robot.speech.speak('I am now raising my arm')
arm.resolve()._send_joint_trajectory([[-1.1, 0.044, 0.80, 0.297, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.9656259917))
ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01, 0.1)).execute({})
arm.resolve().wait_for_motion_done()
arm.resolve()._send_joint_trajectory([[-0.92, 0.044, 0.80, 0.497, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
rospy.sleep(1.0)
return 'done'
@cb_interface(outcomes=['done'])
def place_pose(ud):
robot.torso._send_goal([0.35])
robot.torso.wait_for_motion_done()
rospy.sleep(1.0)
return 'done'
@cb_interface(outcomes=['done'])
def open_gripper(ud):
arm.resolve().send_gripper_goal("open", timeout=0)
rospy.sleep(3.0)
robot.speech.speak("This is where I usually place my cup")
return 'done'
@cb_interface(outcomes=['done'])
def move_away(ud):
robot.torso.high()
robot.torso.wait_for_motion_done()
arm.resolve()._send_joint_trajectory([[-1.1, 0.044, 0.80, 0.297, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'map'
goal_pose.pose.position.x = -4.47331285184
goal_pose.pose.position.y = 5.25921160575
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.0))
ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01, 0.1)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def reset_arms(ud):
robot.torso.reset()
arm.resolve().reset()
robot.torso.wait_for_motion_done()
arm.resolve().wait_for_motion_done()
return 'done'
with self:
self.add_auto('PRE_PLACE_POSE', CBState(pre_place_pose), ['done'])
self.add_auto('PLACE_POSE', CBState(place_pose), ['done'])
self.add_auto('OPEN_GRIPPER', CBState(open_gripper), ['done'])
self.add_auto('MOVE_AWAY', CBState(move_away), ['done'])
self.add('RESET_ARMS', CBState(reset_arms), transitions={'done': 'succeeded'})
def __init__(self):
StateMachine.__init__(self, output_keys=['text'], outcomes=['succeeded', 'aborted', 'preempted'])
self.userdata.text = ''
self.userdata.max_questions = 26
self.userdata.n_questions = 0
self.userdata.max_repeats = 3
self.userdata.n_repeats = 0
list_thinking = ['Let me think', 'Let me see', 'So', 'I think I am getting near', 'Uff, that is difficult', 'well',
'I do not know what to ask you now', 'I think I am getting near.', 'Wait a second', 'Wait a moment',
'Well. Hum', 'Um. Well', 'No clue about it', 'I think I may know it', 'Well, maybe is this.']
list_repeating = ['I did not hear you well, repeat please', 'Repeat your answer please', 'What?',
'Pardon?', 'I beg your pardon, repeat it please', 'Again? What did you say?', 'Tell me again, please',
'Which is your answer?', 'Repeat the answer please', 'Did you say something?', 'And again, your answer is?']
with self:
# TODO integrate movements, timeout, unsuscribing from /word_recognized and maximum questions to WIN/LOSE
StateMachine.add('QUESTION',
AkinatorRequestQuestion(),
transitions={'succeeded': 'GET_USER_ANSWER', 'aborted': 'succeeded'},
remapping={'user_answer': 'text', 'text': 'text'})
StateMachine.add('GET_USER_ANSWER',
SpeechRecognitionAndGesture(),
transitions={'succeeded': 'THINKING',
'aborted': 'REPEATCOUNT'},
remapping={'text': 'text'}
)
ThinkingPool = ['CIR_Thinking1', 'CIR_Thinking2', 'CIR_Thinking3',
'CIR_Thinking4', 'CIR_Thinking5', 'CIR_Thinking6',
'CIR_Thinking7', 'CIR_Thinking8', 'CIR_Thinking9',
'CIR_Thinking10', 'CIR_Thinking11']
StateMachine.add('THINKING',
SpeechGesture(textpool=list_thinking,
behavior_pool=ThinkingPool, wait_before_speak=2.5),
#ExecuteBehaviorFromPoolSM(behavior_pool=['CIR_Thinking1','CIR_Thinking2','CIR_Thinking3','CIR_Thinking4','CIR_Thinking5','CIR_Thinking6']),
transitions={'succeeded': 'REPEATRESET'})
StateMachine.add('QUESTIONSCOUNT',
#CountdownController(self.userdata.max_questions),
CBState(self.countdown_cb,
input_keys=['count', 'max'],
output_keys=['count'],
outcomes=['succeeded', 'aborted']),
transitions={'succeeded': 'QUESTION', 'aborted': 'aborted'},
remapping={'count': 'n_questions', 'max': 'max_questions'}
)
StateMachine.add('REPEAT',
SpeechGesture(textpool=list_repeating, behavior_pool=['CIR_AskingAgain1', 'CIR_AskingAgain2', 'CIR_AskingAgain3']),
#SpeechGesture(text='I did not listen you. Can your repeat?', behavior_name='CIR_AskingAgain1'),
transitions={'succeeded': 'GET_USER_ANSWER'})
StateMachine.add('REPEATCOUNT',
CBState(self.countdown_cb,
input_keys=['count', 'max'],
output_keys=['count'],
outcomes=['succeeded', 'aborted']),
transitions={'succeeded': 'REPEAT', 'aborted': 'aborted'},
remapping={'count': 'n_repeats', 'max': 'max_repeats'})
StateMachine.add('REPEATRESET',
CBState(self.reset_cb, output_keys=['var'], outcomes=['succeeded']),
transitions={'succeeded': 'QUESTIONSCOUNT'},
remapping={'var': 'n_repeats'})
