def __init__(self):
smach.StateMachine.__init__(self, [succeeded, preempted, aborted])
with self:
# move arm
filename = MOTION_FOLDER_PATH + "hang.xml"
hang_motion_goal = MotionManagerGoal()
hang_motion_goal.plan = False
hang_motion_goal.filename = filename
hang_motion_goal.checkSafety = False
hang_motion_goal.repeat = False
hang_motion_action = SimpleActionState('/motion_manager', MotionManagerAction,
goal=hang_motion_goal)
smach.StateMachine.add('MOVE_TO_HANG_POSE', hang_motion_action,
transitions={succeeded: 'RELEASE_OBJECT_FROM_HAND1',
preempted: preempted,
aborted: aborted})
# release
smach.StateMachine.add(
'RELEASE_OBJECT_FROM_HAND1',
OpenReemHand(),
transitions={succeeded: 'MOVE_TO_FINISH_POSE', preempted: preempted, aborted: aborted})
smach.StateMachine.add(
'RELEASE_OBJECT_FROM_HAND2',
OpenReemHand2(),
transitions={succeeded: 'MOVE_TO_FINISH_POSE', preempted: preempted, aborted: aborted})
#move arm home
filename = MOTION_FOLDER_PATH + "final.xml"
finish_motion_goal = MotionManagerGoal()
finish_motion_goal.plan = False
finish_motion_goal.filename = filename
finish_motion_goal.checkSafety = False
finish_motion_goal.repeat = False
finish_motion_action = SimpleActionState('/motion_manager', MotionManagerAction,
goal=finish_motion_goal)
smach.StateMachine.add('MOVE_TO_FINISH_POSE', finish_motion_action,
transitions={succeeded: 'RELEASE_OBJECT_FROM_HAND3',
preempted: preempted,
aborted: aborted})
# close gripper
#smach.StateMachine.add(
# 'CLOSE_HAND',
# CloseReemHand(),
# transitions={succeeded: 'MOVE_TO_FINISH_POSE', preempted: preempted, aborted: aborted})
smach.StateMachine.add(
'RELEASE_OBJECT_FROM_HAND3',
OpenReemHand2(),
transitions={succeeded: succeeded, preempted: preempted, aborted: aborted})
def __init__(self):
smach.StateMachine.__init__(self, [succeeded, preempted, aborted])
with self:
# move arm to point position
filename = MOTION_FOLDER_PATH + "point_at.xml"
point_at_motion_goal = MotionManagerGoal()
point_at_motion_goal.plan = False
point_at_motion_goal.filename = filename
point_at_motion_goal.checkSafety = False
point_at_motion_goal.repeat = False
point_at_motion_action = SimpleActionState(
"/motion_manager", MotionManagerAction, goal=point_at_motion_goal
)
smach.StateMachine.add(
"POINT",
point_at_motion_action,
transitions={succeeded: "OPEN_HAND", preempted: preempted, aborted: aborted},
)
# open
smach.StateMachine.add(
"OPEN_HAND",
OpenReemHand2(),
transitions={succeeded: "TELL_HERE", preempted: preempted, aborted: aborted},
)
# Tell pointed
smach.StateMachine.add(
"TELL_HERE",
SpeakActionState(text="Here it is"),
transitions={succeeded: "MOVE_TO_FINISH_POSE", preempted: preempted, aborted: aborted},
)
# move arm home
filename = MOTION_FOLDER_PATH + "final_pose_point_at.xml"
finish_motion_goal = MotionManagerGoal()
finish_motion_goal.plan = False
finish_motion_goal.filename = filename
finish_motion_goal.checkSafety = False
finish_motion_goal.repeat = False
finish_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=finish_motion_goal)
smach.StateMachine.add(
"MOVE_TO_FINISH_POSE",
finish_motion_action,
transitions={succeeded: succeeded, preempted: preempted, aborted: aborted},
)
def __init__(self, filename, text, checksafety=True, plan=True, wait_before_speaking=0):
smach.Concurrence.__init__(self, outcomes=['succeeded', 'aborted'],
default_outcome='aborted',
outcome_map={'succeeded': {'MOTION_ACTION': 'succeeded',
'SPEAK_ACTION': 'succeeded'}})
with self:
motion_goal = MotionManagerGoal()
motion_goal.plan = plan
motion_goal.filename = filename
motion_goal.checkSafety = checksafety
motion_goal.repeat = False
smach.Concurrence.add('MOTION_ACTION', SimpleActionState('/motion_manager', MotionManagerAction,
goal=motion_goal))
smach.Concurrence.add('SPEAK_ACTION', SpeakActionState(text=text, wait_before_speaking=wait_before_speaking))
def __init__(self):
smach.StateMachine.__init__(self, [succeeded, preempted, aborted])
with self:
# move arms to cool pose
filename = MOTION_FOLDER_PATH + "cool_cloth_pose.xml"
cool_motion_goal = MotionManagerGoal()
cool_motion_goal.plan = False
cool_motion_goal.filename = filename
cool_motion_goal.checkSafety = False
cool_motion_goal.repeat = False
cool_motion_action = SimpleActionState('/motion_manager', MotionManagerAction,
goal=cool_motion_goal)
smach.StateMachine.add('MOVE_TO_COOL_POSE', cool_motion_action,
transitions={succeeded: succeeded,
preempted: preempted,
aborted: aborted})
def arms_out_of_self_colision(self):
self._print_title("ARMS OUT OF SELF COLISION")
if self.using_the_robot:
if self.__check_action(ARMS_ACTION_NAME) == aborted:
return aborted
else:
self._print_warning("Not checking. ROS_MASTER_URI nor COMPUTER_NAME contains %s" % ROBOTS_NAME)
return aborted
robot = os.environ.get('PAL_ROBOT')
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
rospy.loginfo('remotelly_executing: %s' % remotelly_executing) # FIXME: Remove this line after test in the robot
MOTION_FOLDER_PATH = ''
plan = False
checkSafety = False
if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3' or remotelly_executing == -1:
check_for_door = True # FIXME: This variable is not being used.
MOTION_FOLDER_PATH = "/mnt_flash/stacks/reem_alive/alive_engine/config/database/Stopped/interact_1.xml"
if remotelly_executing == -1:
MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml'
plan = True
checkSafety = False
motion_goal = MotionManagerGoal()
motion_goal.plan = plan
motion_goal.filename = MOTION_FOLDER_PATH
motion_goal.checkSafety = checkSafety
motion_goal.repeat = False
motion_manager_action = SimpleActionState(ARMS_ACTION_NAME, MotionManagerAction, goal=motion_goal)
userdata_hacked = {}
status = motion_manager_action.execute(userdata_hacked)
if status == succeeded:
rospy.loginfo(self.colors.GREEN_BOLD + "Arms out of self colisin: OK " + self.colors.NATIVE_COLOR)
else:
self.ALL_OK = False
rospy.loginfo(self.colors.RED_BOLD + "Arms out of self colisin: Failed " + self.colors.NATIVE_COLOR)
def main():
rospy.init_node("sm_pick_up_cloths")
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=["FINISH_OK", "FAILED_VISION", "FAILED_MOVE_ARM", "FAILED_MOVE_BASE"])
# Open the container
with sm:
# Add states to the container
# smach.StateMachine.add('GetPCL', GetPCL('/camera/rgb/points'),
# TODO SALUDO
# TODO RETRYING
smach.StateMachine.add(
"GetPCL",
GetPCL("/head_mount_xtion/depth_registered/points"),
transitions={"success": "GetGraspingPointAction", "fail": "FAILED_VISION"},
remapping={"pcl_RGB": "pcl_RGB"},
)
smach.StateMachine.add(
"GetGraspingPointAction",
SimpleActionState(
"/skills/bow_detector/bow_object_detector/get_grasping_point",
GetGraspingPointAction,
# goal_slots=['pcl_RGB'],
goal_cb=grasping_point_goal_cb,
input_keys=["pcl_RGB"],
result_slots=["grasping_point"],
),
# server_wait_timeout=rospy.Duration(10.0)),
transitions={"succeeded": "ConvertGraspPoint", "preempted": "FAILED_VISION", "aborted": "FAILED_VISION"},
remapping={"grasping_point": "sm_grasp_point"},
)
# prepare pose
smach.StateMachine.add(
"ConvertGraspPoint",
CBState(build_grasp_pose_from_point_cb),
transitions={"done": "TransformPoseStamped"},
remapping={
"grasp_point": "sm_grasp_point",
"grasp_pose_st": "sm_grasp_pose",
"target_frame": "sm_target_frame",
},
)
# transform pose ¿needed?
smach.StateMachine.add(
"TransformPoseStamped",
TransformPoseStamped(),
transitions={
"succeeded": "ReemGraspStateMachine",
"preempted": "FAILED_VISION",
"aborted": "FAILED_VISION",
},
remapping={"target_pose": "target_pose_stamped"},
)
# grasp
smach.StateMachine.add(
"ReemGraspStateMachine",
SMReemClothGraspStateMachine(),
transitions={"succeeded": "MOVE_TO_COOL_POSE", "preempted": "FAILED_MOVE_ARM", "aborted": "GetPCL"},
)
# move arms to cool pose
filename = MOTION_FOLDER_PATH + "cool_cloth_pose.xml"
cool_motion_goal = MotionManagerGoal()
cool_motion_goal.plan = True
cool_motion_goal.filename = filename
cool_motion_goal.checkSafety = True
cool_motion_goal.repeat = False
cool_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=cool_motion_goal)
smach.StateMachine.add(
"MOVE_TO_COOL_POSE",
cool_motion_action,
transitions={"succeeded": "MOVE_TO_HANGER", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM"},
)
# move to hanger
sm.userdata.hanger = HANGER_NAME
smach.StateMachine.add(
"MOVE_TO_HANGER",
MoveToRoomStateMachine(announcement=None),
transitions={
"succeeded": "MOVE_TO_HANG_POSE",
"preempted": "FAILED_MOVE_BASE",
"aborted": "FAILED_MOVE_BASE",
},
remapping={"room_name": "hanger"},
)
# move arm
filename = MOTION_FOLDER_PATH + "hang.xml"
hang_motion_goal = MotionManagerGoal()
hang_motion_goal.plan = True
hang_motion_goal.filename = filename
hang_motion_goal.checkSafety = True
hang_motion_goal.repeat = False
hang_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=hang_motion_goal)
smach.StateMachine.add(
"MOVE_TO_HANG_POSE",
hang_motion_action,
transitions={
"succeeded": "RELEASE_OBJECT_FROM_HAND",
"preempted": "FAILED_MOVE_ARM",
"aborted": "FAILED_MOVE_ARM",
},
)
# release
smach.StateMachine.add(
"RELEASE_OBJECT_FROM_HAND",
OpenReemHand(),
transitions={"succeeded": "CLOSE_HAND", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM"},
)
# close gripper
smach.StateMachine.add(
"CLOSE_HAND",
CloseReemHand(),
transitions={
"succeeded": "MOVE_TO_FINISH_POSE",
"preempted": "FAILED_MOVE_ARM",
"aborted": "FAILED_MOVE_ARM",
},
)
# move arm home
filename = MOTION_FOLDER_PATH + "final.xml"
finish_motion_goal = MotionManagerGoal()
finish_motion_goal.plan = True
finish_motion_goal.filename = filename
finish_motion_goal.checkSafety = True
finish_motion_goal.repeat = False
finish_motion_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=finish_motion_goal)
smach.StateMachine.add(
"MOVE_TO_FINISH_POSE",
finish_motion_action,
transitions={"succeeded": "FINISH_OK", "preempted": "FAILED_MOVE_ARM", "aborted": "FAILED_MOVE_ARM"},
)
# finish
# Execute SMACH plan
outcome = sm.execute()
import rospy
import actionlib
from std_srvs.srv import Empty, EmptyResponse
from pal_control_msgs.msg import MotionManagerGoal, MotionManagerAction
def hand_initialize(name):
rospy.wait_for_service(name)
try:
hand_init = rospy.ServiceProxy(name, Empty)
resp1 = hand_init()
return resp1
except rospy.ServiceException, e:
print "Service call failed: %s"%e
if __name__ == "__main__":
rospy.init_node('calibrator')
client = actionlib.SimpleActionClient('/motion_manager', MotionManagerAction)
client.wait_for_server()
goal = MotionManagerGoal()
goal.filename = '/mnt_flash/etc/control/robot/reemh3/motion/start_reem.xml'
goal.checkSafety = True
goal.repeat = False
goal.priority = 0
goal.queueTimeout = 0
client.send_goal(goal)
client.wait_for_result()
hand_initialize ('/rightHandDeviceInitializerStart')
hand_initialize ('/leftHandDeviceInitializerStart')
def __init__(self, distance=1.5, orient_after_passing=math.pi/2, openDoor=False):
"""
Look for a door, open it (if needed) and enter the room.
:param distance: The distance in meters to go into the room.
:param orientation: 0 to look to the front, 1 to rotate towards the door
:return output_keys: the position to the door (in abs. coordinates)
"""
smach.StateMachine.__init__(self, [succeeded, preempted, aborted])
self.door_position = -1
with self:
'''if openDoor:
smach.StateMachine.add('SM_OPEN_DOOR',
EnterDoorStateMachine(),
transitions={succeeded: succeeded, aborted: 'SAY_CANT_OPEN'})
smach.StateMachine.add('SAY_CANT_OPEN', SpeakActionState(text="I can't open the door. Can you please open it for me?"),
transitions={succeeded: 'CROSSING_DOOR_POS', aborted: 'CROSSING_DOOR_POS'})'''
if check_for_door:
plan = True
checkSafety = False
rest_goal = MotionManagerGoal()
rest_goal.plan = plan
rest_goal.filename = MOTION_FOLDER_PATH + 'rest.xml'
rest_goal.checkSafety = checkSafety
rest_goal.repeat = False
smach.StateMachine.add(
"STOP_ROBOT_CONTROLLERS",
StopRobotControllers(head=True, left=True, right=True),
transitions={succeeded: "DISABLE_REEM_ALIVE", preempted: "DISABLE_REEM_ALIVE", aborted: "DISABLE_REEM_ALIVE" }
) # if aborts maybe is because they are already stopped.
smach.StateMachine.add('DISABLE_REEM_ALIVE',
ServiceState('/alive_engine/stop',
Empty),
transitions={succeeded: 'CROSSING_DOOR_POS'})
smach.StateMachine.add('CROSSING_DOOR_POS', SimpleActionState('/motion_manager', MotionManagerAction,
goal=rest_goal),
transitions={succeeded: 'CHECK_CAN_PASS', aborted: 'CROSSING_DOOR_POS'})
@smach.cb_interface(outcomes=[succeeded, aborted, preempted, 'door_too_far'])
def check_range(userdata, message):
length_ranges = len(message.ranges)
alpha = math.atan((self.WIDTH/2)/distance)
n_elem = int(math.ceil(length_ranges*alpha/(2*message.angle_max))) # Num elements from the 0 angle to the left or right
middle = length_ranges/2
cut = [x for x in message.ranges[middle-n_elem:middle+n_elem] if x > 0.01]
print "cut: " + str(cut)
minimum = min(cut)
if self.door_position == -1:
if message.ranges[middle] <= self.MAX_DIST_TO_DOOR:
self.door_position = message.ranges[middle]
else:
return 'door_too_far'
if (minimum >= distance+self.door_position):
return succeeded
rospy.loginfo("Distance in front of the robot is too small: " + str(distance+self.door_position) + ". Minimum distance: " + str(minimum))
return aborted
smach.StateMachine.add('CHECK_CAN_PASS',
TopicReaderState(topic_name=LASER_TOPIC,
msg_type=LaserScan,
timeout=self.READ_TIMEOUT,
callback=check_range,
outcomes=[succeeded, aborted, preempted, 'door_too_far']),
remapping={'message': 'range_readings'},
transitions={succeeded: 'ENTER_ROOM',
aborted: 'CHECK_CAN_PASS',
'door_too_far': 'SAY_TOO_FAR_FROM_DOOR'})
smach.StateMachine.add('SAY_TOO_FAR_FROM_DOOR', SpeakActionState(text="I'm too far from the door."),
transitions={succeeded: 'CHECK_CAN_PASS', aborted: 'CHECK_CAN_PASS'})
def move_enter_room_cb(userdata, nav_goal):
pose = Pose()
pose.position = Point(distance+self.door_position, 0, 0)
pose.orientation = Quaternion(*quaternion_from_euler(0, 0, orient_after_passing))
nav_goal.target_pose.pose = transform_pose(pose, 'base_link', 'map')
return nav_goal
smach.StateMachine.add('ENTER_ROOM',
MoveActionState("/map", "/move_base", goal_cb=move_enter_room_cb),
transitions={succeeded: 'INTERACT_POS', aborted: 'ENTER_ROOM'} if check_for_door else {aborted: 'ENTER_ROOM'})
# transitions={succeeded: 'succeeded', aborted: 'ENTER_ROOM'} if check_for_door else {aborted: 'ENTER_ROOM'})
if check_for_door: # It's safer to navigate with the arms like that
interact_goal = MotionManagerGoal()
interact_goal.plan = plan
interact_goal.checkSafety = checkSafety
interact_goal.repeat = False
interact_goal.filename = MOTION_FOLDER_PATH + 'interact.xml'
smach.StateMachine.add('INTERACT_POS',
SimpleActionState('/motion_manager', MotionManagerAction, goal=interact_goal),
transitions={succeeded: succeeded})
def __init__(self):
smach.StateMachine.__init__(self, [succeeded, preempted, aborted])
with self:
# desactivate controllers
smach.StateMachine.add(
'DeactivateControllersInit',
StopRobotControllers(head=True, left=True, right=True),
transitions={succeeded: 'MOVE_TO_NO_COLLISION_POSE', preempted: preempted, aborted: aborted})
# move arms to cool pose
filename = MOTION_FOLDER_PATH + "interact_head_up.xml"
cool_motion_goal = MotionManagerGoal()
cool_motion_goal.plan = False
cool_motion_goal.filename = filename
cool_motion_goal.checkSafety = False
cool_motion_goal.repeat = False
cool_motion_action = SimpleActionState('/motion_manager', MotionManagerAction,
goal=cool_motion_goal)
smach.StateMachine.add('MOVE_TO_NO_COLLISION_POSE', cool_motion_action,
transitions={succeeded: 'GetPCL',
preempted: preempted,
aborted: succeeded})
smach.StateMachine.add('GetPCL', GetPCL('/head_mount_xtion/depth_registered/points'),
transitions={'success':'GetGraspingPointAction',
'fail':aborted},
remapping={'pcl_RGB':'pcl_RGB'})
smach.StateMachine.add('GetGraspingPointAction',
SimpleActionState('/skills/bow_detector/bow_object_detector/get_grasping_point',
GetGraspingPointAction,
#goal_slots=['pcl_RGB'],
goal_cb=grasping_point_goal_cb,
input_keys=['pcl_RGB'],
result_slots=['grasping_point']),
#server_wait_timeout=rospy.Duration(10.0)),
transitions={'succeeded':'ConvertGraspPoint',
'preempted':preempted,
'aborted':aborted},
remapping={'grasping_point':'sm_grasp_point'})
# prepare pose
smach.StateMachine.add('ConvertGraspPoint',
CBState(build_grasp_pose_from_point_cb),
transitions={'done':'TransformPoseStamped'},
remapping={'grasp_point' : 'sm_grasp_point',
'grasp_pose_st' : 'sm_grasp_pose',
'target_frame' : 'sm_target_frame'})
# transform pose ¿needed?
smach.StateMachine.add('TransformPoseStamped',
TransformPoseStamped(),
transitions={'succeeded':'ActivateControllers',
'preempted':preempted,
'aborted':aborted},
remapping={'target_pose':'target_pose_stamped'})
# activate controllers
smach.StateMachine.add(
'ActivateControllers',
StartRobotControllers(head=False, left=False, right=True),
transitions={succeeded: 'ReemGraspStateMachine', preempted: preempted, aborted: aborted})
# grasp
smach.StateMachine.add(
'ReemGraspStateMachine',
SMReemClothGraspStateMachine(),
transitions={'succeeded': 'DeactivateControllers', 'preempted': preempted, 'aborted': aborted})
# desactivate controllers
smach.StateMachine.add(
'DeactivateControllers',
StopRobotControllers(head=False, left=False, right=True),
transitions={succeeded: succeeded, preempted: preempted, aborted: aborted})
def __init__(self, distToHuman=0.9):
smach.StateMachine.__init__(
self,
outcomes=[succeeded, preempted, aborted],
input_keys=["in_learn_person"])
with self:
if remotelly_executing == -1 or inside_robot:
'''def disable_cb(userdata, request):
disableEmerg = DisableEmergencyRequest()
disableEmerg.second = 600
disableEmerg.useLEDs = True
return disableEmerg
smach.StateMachine.add('DISABLE_EMERGENCY_STOP',
ServiceState('/disable_emergency_stop',
DisableEmergency,
request_cb=disable_cb),
transitions={succeeded: 'DISABLE_REEM_ALIVE'})
smach.StateMachine.add('DISABLE_REEM_ALIVE',
ServiceState('/alive_engine/stop', Empty),
transitions={succeeded: 'DISABLE_FACE_TRACKING'})'''
smach.StateMachine.add('DISABLE_FACE_TRACKING',
ServiceState('/personServer/faceTracking/stop', Empty),
transitions={succeeded: 'START_HEAD_CONTROLLERS'})
smach.StateMachine.add(
"START_HEAD_CONTROLLERS",
StartRobotControllers(head=True, left=False, right=False),
transitions={succeeded: "CROSSING_DOOR_POS", aborted: "START_HEAD_CONTROLLERS", preempted: "START_HEAD_CONTROLLERS"})
motion_goal = MotionManagerGoal()
motion_goal.plan = False
motion_goal.filename = MOTION_FOLDER_PATH
motion_goal.checkSafety = False
motion_goal.repeat = False
smach.StateMachine.add('CROSSING_DOOR_POS',
SimpleActionState('/motion_manager', MotionManagerAction, goal=motion_goal),
transitions={succeeded: 'FIX_HEAD_POSITION',
aborted: 'FIX_HEAD_POSITION'})
@smach.cb_interface(outcomes=[succeeded, preempted, aborted])
def fixHeadPosition(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
head_goal.target.point.x = 1.0
head_goal.target.point.y = 0.0
head_goal.target.point.z = 1.65
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(5.0))
client.send_goal(head_goal)
return succeeded
smach.StateMachine.add('FIX_HEAD_POSITION',
CBState(fixHeadPosition),
transitions={succeeded: "LEARN_PERSON",
preempted: "LEARN_PERSON",
aborted: "LEARN_PERSON"})
smach.StateMachine.add('LEARN_PERSON',
LearnPerson(),
transitions={succeeded: 'TRACK_OPERATOR',
aborted: aborted},
remapping={'out_personTrackingData': 'out_personTrackingData',
"in_learn_person": "in_learn_person"})
smach.StateMachine.add('TRACK_OPERATOR',
TrackOperator(distToHuman),
remapping={'in_personTrackingData': 'out_personTrackingData'},
transitions={succeeded: succeeded,
preempted: preempted,
aborted: aborted})
def main():
rospy.init_node("robot_inspection")
# Create a SMACH state machine
sm = smach.StateMachine(["succeeded", "aborted", "preempted"])
with sm: # FIXME Do all the speak actions and motion actions simultanously??
smach.StateMachine.add(
"CLOSE_HAND", CloseReemHand(), transitions={"succeeded": "STARTING_SPEECH", "aborted": "STARTING_SPEECH"}
)
## Indicate that we are starting
smach.StateMachine.add(
"STARTING_SPEECH",
SpeakActionState(
text="I am ready to start the registration. I will now go to the registration desk.",
wait_before_speaking=Duration(1.0),
),
transitions={"succeeded": "ENTER_ROOM", "aborted": "ENTER_ROOM"},
)
# Enters into the room through the door
smach.StateMachine.add(
"ENTER_ROOM",
EnterRoomStateMachine(distance=DOOR_DISTANCE, orient_after_passing=0.0),
transitions={"succeeded": "ENABLE_REEM_ALIVE"},
)
smach.StateMachine.add(
"ENABLE_REEM_ALIVE",
ServiceState("/alive_engine/start", Empty),
transitions={"succeeded": "MOVE_TO_REG_TABLE", "aborted": "MOVE_TO_REG_TABLE"},
)
# Go to table
sm.userdata.registration_table = REGISTRATION_TABLE_NAME
smach.StateMachine.add(
"MOVE_TO_REG_TABLE",
MoveToRoomStateMachine(announcement=None),
transitions={"succeeded": "DISABLE_REEM_ALIVE", "aborted": "MOVE_TO_REG_TABLE"},
remapping={"room_name": "registration_table"},
)
smach.StateMachine.add(
"DISABLE_REEM_ALIVE",
ServiceState("/alive_engine/stop", Empty),
transitions={"succeeded": "MOVE_TO_BOW", "aborted": "MOVE_TO_BOW"},
)
# Bow to face
filename = MOTION_FOLDER_PATH + "bow.xml"
bow_motion_goal = MotionManagerGoal()
bow_motion_goal.plan = False
bow_motion_goal.filename = filename
bow_motion_goal.checkSafety = False
bow_motion_goal.repeat = False
bow_motion_goal.priority = 0
bow_action = SimpleActionState("/motion_manager", MotionManagerAction, goal=bow_motion_goal)
smach.StateMachine.add(
"MOVE_TO_BOW", bow_action, transitions={"succeeded": "PRESENT_REEM", "aborted": "PRESENT_REEM"}
)
# Present the robot and the team
present_text = "Hi everybody! My name is REEM. I came here with my teammates from the REEM_@_EERI team!"
smach.StateMachine.add(
"PRESENT_REEM",
SpeakActionState(text=present_text, wait_before_speaking=Duration(0.2)),
transitions={"succeeded": "START_PRESENT", "aborted": "PRESENT_REEM"},
)
# Deliver the form and say something funny
take_it_text = "Oh, you look really good today! This is my registration form. Please, take it."
smach.StateMachine.add(
"SAY_HERE_CV",
SpeakActionState(text=take_it_text, wait_before_speaking=Duration(0.10)),
transitions={"succeeded": "OPEN_HAND", "aborted": "SAY_HERE_CV"},
)
smach.StateMachine.add(
"START_PRESENT",
SpeakAndMotionActionConcurrentSM(
filename=MOTION_FOLDER_PATH + "robocup_start_presentation.xml",
checksafety=False,
plan=False,
text=take_it_text,
),
transitions={"succeeded": "OPEN_HAND", "aborted": "SAY_HERE_CV"},
)
smach.StateMachine.add(
"OPEN_HAND", OpenReemHand2(), transitions={"succeeded": "SAY_READ_AND_MID", "aborted": "SAY_READ_AND_MID"}
)
read_this = "Please, can you read the form and tell my mates if there's anything wrong."
smach.StateMachine.add(
"SAY_READ_AND_MID",
SpeakAndMotionActionConcurrentSM(
filename=MOTION_FOLDER_PATH + "robocup_middle_presentation.xml",
checksafety=False,
plan=False,
text=read_this,
wait_before_speaking=Duration(1.0),
),
transitions={"succeeded": "END_PRESENTATION_AND_THANK", "aborted": "SAY_HERE_CV"},
)
thank_you = "Thank you. Tell me when I should leave the arena."
smach.StateMachine.add(
"END_PRESENTATION_AND_THANK",
SpeakAndMotionActionConcurrentSM(
filename=MOTION_FOLDER_PATH + "robocup_end_presentation.xml",
checksafety=False,
plan=False,
text=thank_you,
wait_before_speaking=Duration(0.8),
),
transitions={"succeeded": "RE_ENABLE_REEM_ALIVE", "aborted": "SAY_HERE_CV"},
)
smach.StateMachine.add(
"RE_ENABLE_REEM_ALIVE",
ServiceState("/alive_engine/start", Empty),
transitions={"succeeded": "WAIT_LISTEN_ORDER", "aborted": "WAIT_LISTEN_ORDER"},
)
smach.StateMachine.add(
"WAIT_LISTEN_ORDER",
RecogCommand(
GRAMMAR_NAME="robocup/room",
command_key="location",
command_value=LEAVING_DOOR_NAME,
ask_for_confirmation=False,
),
transitions={"succeeded": "BABY_DONT_HURT_ME", "aborted": "ASK_TO_REPEAT"},
)
repeat_pool = [
"Pardon?",
"Can you repeat the name, please?",
"I'm sorry I didn't understand you. Can you repeat that?",
"I didn't get it. Can you please repeat?",
]
smach.StateMachine.add(
"ASK_TO_REPEAT",
SpeakActionFromPoolStateMachine(repeat_pool),
transitions={"succeeded": "WAIT_LISTEN_ORDER", "aborted": "WAIT_LISTEN_ORDER"},
)
baby_dont_hurt_me = "Okay. Please, Don't hurt me when you hit the emergency button."
smach.StateMachine.add(
"BABY_DONT_HURT_ME",
SpeakActionState(text=baby_dont_hurt_me, wait_before_speaking=Duration(0.20)),
transitions={"succeeded": "SAY_AND_DO_BYE", "aborted": "SAY_HERE_CV"},
)
# Say Bye bye and move the hand
smach.StateMachine.add(
"SAY_AND_DO_BYE",
SpeakAndMotionActionConcurrentSM(
filename=MOTION_FOLDER_PATH + "wave.xml",
checksafety=False,
plan=True,
text="Bye Bye!",
wait_before_speaking=Duration(2.1),
),
transitions={"succeeded": "INTERACT_POS", "aborted": "INTERACT_POS"},
)
interact_goal = MotionManagerGoal()
interact_goal.plan = True
interact_goal.filename = filename
interact_goal.checkSafety = False
interact_goal.repeat = False
interact_goal.priority = 0
interact_goal.filename = MOTION_FOLDER_PATH + "interact.xml"
smach.StateMachine.add(
"INTERACT_POS",
SimpleActionState("/motion_manager", MotionManagerAction, goal=interact_goal),
transitions={"succeeded": "SLOW_SPEED", "aborted": "INTERACT_POS"},
)
def slow_speed(userdata):
node_to_reconfigure = "/move_base/PalLocalPlanner"
client = dynamic_reconfigure.client.Client(node_to_reconfigure)
new_params = {"max_vel_x": 0.2}
client.update_configuration(new_params)
return "succeeded"
smach.StateMachine.add(
"SLOW_SPEED", smach.CBState(slow_speed, outcomes=["succeeded"]), transitions={"succeeded": "GO_PRE_EXIT"}
)
# Go to the leaving door
sm.userdata.pre_exit_door = PRE_EXIT_NAME
smach.StateMachine.add(
"GO_PRE_EXIT",
MoveToRoomStateMachine(announcement=None),
transitions={"succeeded": "STOP_REEM_ALIVE", "aborted": "CANT_LEAVE"},
remapping={"room_name": "pre_exit_door"},
)
smach.StateMachine.add(
"STOP_REEM_ALIVE",
ServiceState("/alive_engine/stop", Empty),
transitions={"succeeded": "CROSSING_DOOR_POS", "aborted": "CROSSING_DOOR_POS"},
)
rest_goal = MotionManagerGoal()
rest_goal.plan = True
rest_goal.filename = filename
rest_goal.checkSafety = False
rest_goal.repeat = False
rest_goal.priority = 0
rest_goal.filename = MOTION_FOLDER_PATH + "rest.xml"
smach.StateMachine.add(
"CROSSING_DOOR_POS",
SimpleActionState("/motion_manager", MotionManagerAction, goal=rest_goal),
transitions={"succeeded": "LEAVE_ARENA", "aborted": "CROSSING_DOOR_POS"},
)
sm.userdata.exit_door = LEAVING_DOOR_NAME
smach.StateMachine.add(
"LEAVE_ARENA",
MoveToRoomStateMachine(announcement=None),
transitions={"succeeded": "succeeded", "aborted": "LEAVE_ARENA"},
remapping={"room_name": "exit_door"},
)
cant_leave = "Emergency button was pressed, retrying to leave the arena."
smach.StateMachine.add(
"CANT_LEAVE",
SpeakActionState(text=cant_leave, wait_before_speaking=Duration(0.0)),
transitions={"succeeded": "GO_PRE_EXIT", "aborted": "GO_PRE_EXIT"},
)
# Create and start the introspection server
sis = smach_ros.IntrospectionServer("server_name", sm, "/SM_ROOT")
sis.start()
# Execute the state machine
sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
