def main():
rospy.init_node('drone_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome'])
sm.userdata.lspeed = 0.5
sm.userdata.rspeed = 1.2
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('TAKEOFF', CBState(takeoff_cb), {
'finished': 'LAND',
'failed': 'LAND'
})
smach.StateMachine.add('LAND', CBState(land_cb), {
'finished': 'outcome',
'failed': 'outcome'
})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('drone_server', sm, '/SM_DRONE')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
sis.stop()
def construct_sm():
sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
sm.userdata.nums = range(25, 88, 3)
sm.userdata.even_nums = []
sm.userdata.odd_nums = []
with sm:
## %Tag(ITERATOR)%
tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
input_keys = ['nums', 'even_nums', 'odd_nums'],
it = lambda: range(0, len(sm.userdata.nums)),
output_keys = ['even_nums', 'odd_nums'],
it_label = 'index',
exhausted_outcome = 'succeeded')
## %EndTag(ITERATOR)%
## %Tag(CONTAINER)%
with tutorial_it:
container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
output_keys = ['even_nums', 'odd_nums'])
with container_sm:
#test wether even or odd
StateMachine.add('EVEN_OR_ODD',
ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
input_keys=['nums', 'index']),
{'true':'ODD',
'false':'EVEN' })
#add even state
@smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('EVEN', CBState(even_cb),
{'succeeded':'continue'})
#add odd state
@smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return 'succeeded'
StateMachine.add('ODD', CBState(odd_cb),
{'succeeded':'continue'})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
#close container_sm
Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['continue'])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
#close the tutorial_it
StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'succeeded',
'aborted':'aborted'})
## %EndTag(ADDITERATOR)%
return sm
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, 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, panda_arm, pick_pose, place_pose, hover_distance = 0.0, num_retries=2):
self.panda_arm = panda_arm
self.num_retries = num_retries
self.pick_pose = pick_pose
self.place_pose = place_pose
#Create a SMACH state machine
self.sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted'])
self.sm.userdata.sm_pick_object_pose = Pose()
self.sm.userdata.sm_place_object_pose = Pose()
self.sm.userdata.sm_hover_distance = hover_distance
with self.sm:
smach.StateMachine.add('SET_PICK_POSE', CBState(self.set_pick_place_pose, cb_args=[self]),
transitions={'succeeded':'MOVE_TO_PICK_HOVER_POSE', 'aborted': 'aborted'},
remapping={'pick_object_pose':'sm_pick_object_pose',
'place_object_pose':'sm_place_object_pose',
'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('MOVE_TO_PICK_HOVER_POSE', CBState(self.move_to_pick_hover_pose, cb_args=[self]),
transitions={'succeeded':'MOVE_TO_PICK_POSE', 'failed_hover':'failed', 'aborted': 'aborted'},
remapping={'pick_object_pose':'sm_pick_object_pose',
'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('MOVE_TO_PICK_POSE', CBState(self.move_to_pick_pose, cb_args=[self]),
transitions={'succeeded':'GRASP_AND_HOVER', 'failed_pick_pose': 'failed', 'aborted': 'aborted'},
remapping={'pick_object_pose':'sm_pick_object_pose'})
smach.StateMachine.add('GRASP_AND_HOVER', CBState(self.grasp_and_hover, cb_args=[self]),
transitions={'succeeded':'MOVE_TO_PLACE_HOVER_POSE', 'failed_grasp': 'failed', 'aborted': 'aborted'},
remapping={'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('MOVE_TO_PLACE_HOVER_POSE', CBState(self.move_to_place_hover_pose, cb_args=[self]),
transitions={'succeeded':'MOVE_TO_PLACE_POSE', 'failed_hover': 'failed', 'aborted': 'aborted'},
remapping={'place_object_pose':'sm_place_object_pose',
'hover_distance':'sm_hover_distance'})
smach.StateMachine.add('MOVE_TO_PLACE_POSE', CBState(self.move_to_place_pose, cb_args=[self]),
transitions={'succeeded':'PLACE_AND_HOVER', 'failed_place_pose': 'failed', 'aborted': 'aborted'},
remapping={'place_object_pose':'sm_place_object_pose'})
smach.StateMachine.add('PLACE_AND_HOVER', CBState(self.place_and_hover, cb_args=[self]),
transitions={'succeeded':'done', 'failed_grasp': 'failed', 'aborted': 'aborted'},
remapping={'place_object_pose':'sm_place_object_pose',
'hover_distance':'sm_hover_distance'})
def __init__(self):
Concurrence.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['log_mission'],
output_keys=['log_mission'],
child_termination_cb=self.child_termination_cb,
outcome_map={'succeeded': {
'WAITING': 'finished'
}})
self.register_start_cb(self.start_pause)
self.register_termination_cb(self.termination_pause)
_min_time_to_go_recharge = 15.0
@cb_interface(input_keys=['log_mission'],
outcomes=['insufficient_time', 'recharge', 'preempted'])
def check_waiting_time(ud):
navigation_duration = datetime.datetime.now(
) - ud.log_mission['start_time']
if datetime.timedelta(seconds=ud.log_mission['patrol']['min_duration']) - navigation_duration \
> datetime.timedelta(seconds=_min_time_to_go_recharge):
return 'recharge'
else:
return 'succeeded'
self.recharge = StateMachine(
input_keys=['log_mission'],
output_keys=['log_mission'],
outcomes=['succeeded', 'aborted', 'preempted'])
with self.recharge:
StateMachine.add('CHECK_WAITING_TIME', CBState(check_waiting_time),
{
'recharge': 'RECHARGE',
'insufficient_time': 'succeeded'
})
StateMachine.add('RECHARGE', Recharge())
with self:
Concurrence.add('WAITING', Waiting())
Concurrence.add('CONDITIONAL_RECHARGE', self.recharge)
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"])
@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 allign_with_target(target):
"""
Returns a state that alligns with the specified target.
target: string on the form 'GATE' or 'BOUY'
The returned state is responsible for chaning the circeling direction when needed.
"""
# TODO: get target position (x,y) from landmark server
# TODO: create a move_goal
move_goal = None
allignment_attempt = Concurrence(
outcomes=['succeeded', 'preempted', 'wrong_direction'],
outcome_map={
'succeeded': {
'ALLIGNMENT_CHECKER': 'alligned'
},
'wrong_direction': {
'ALLIGNMENT_CHECKER': 'wrong_direction'
}
},
default_outcome=['preempted'],
child_termination_cb=None # TODO: should allways terminate
)
with allignment_attempt:
Concurrence.add(
'CIRCLE_GATE',
SimpleActionState('controller/move', MoveAction,
goal=move_goal) # TODO
)
Concurrence.add('ALLIGNMENT_CHECKER', CBState(allignment_checker))
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 setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
state_machine.userdata['item_picked'] = None
with state_machine:
# Intro
# StateMachine.add('START_CHALLENGE_ROBUST',
# Initialize(robot),
# transitions={'initialized': 'SAY_START', 'abort': 'done'})
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, "initial_pose"), # ToDo: in knowledge
transitions={'Done': 'SAY_START',
'Aborted': 'done',
'Failed': 'SAY_START'})
StateMachine.add('SAY_START',
Say(robot,
"Let's set the table baby! If there are any chairs near the kitchen_table, please remove them",
block=False),
transitions={'spoken': 'NAVIGATE_AND_OPEN_CUPBOARD'})
# The pre-work
StateMachine.add('NAVIGATE_AND_OPEN_CUPBOARD',
NavigateToAndOpenCupboard(robot, "kitchen_cabinet", "in_front_of"),
transitions={'succeeded': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
'failed': 'SAY_OPEN_FAILED'})
StateMachine.add('SAY_OPEN_FAILED',
Say(robot, "I failed to open the cupboard drawer"),
transitions={'spoken': 'WAIT_OPEN'})
StateMachine.add('WAIT_OPEN',
WaitTime(robot, 5),
transitions={'waited': 'SAY_OPEN_THANKS', 'preempted': 'done'})
StateMachine.add('SAY_OPEN_THANKS',
Say(robot, "Thank you darling"),
transitions={'spoken': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# The loop
StateMachine.add('NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
NavigateToAndPickItemFromCupboardDrawer(robot, "kitchen_cabinet", "in_front_of",
required_items),
transitions={'succeeded': 'PLACE_ITEM_ON_TABLE',
'failed': 'CHECK_IF_WE_HAVE_IT_ALL'})
StateMachine.add('PLACE_ITEM_ON_TABLE',
NavigateToAndPlaceItemOnTable(robot, "kitchen_table", "right_of", "right_of_close"),
transitions={'succeeded': 'CHECK_IF_WE_HAVE_IT_ALL',
'failed': 'WAIT'})
StateMachine.add('WAIT',
WaitTime(robot, 2),
transitions={'waited': 'CHECK_IF_WE_HAVE_IT_ALL', 'preempted': 'done'})
StateMachine.add('CHECK_IF_WE_HAVE_IT_ALL',
CBState(check_if_we_have_it_all, cb_args=[robot]),
transitions={'we_have_it_all': 'SAY_END_CHALLENGE',
'keep_going': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# Outro
StateMachine.add('SAY_END_CHALLENGE',
Say(robot, "That was all folks, have a nice meal!"),
transitions={'spoken': 'done'})
StateMachine.add('NAVIGATE_AND_CLOSE_CUPBOARD',
NavigateToAndCloseCupboard(robot, "cupboard", "in_front_of"),
transitions={'succeeded': 'done',
'failed': 'done'})
return state_machine
# Start for RecognitionThread
# try:
# rec_and_show = RecognitionThread(0,config.getboolean('GAPL','show_flag'), config.getboolean('GAPL','streaming_flag'),config.getboolean('GAPL','recording_flag'))
# except:
# print 'camera 1 ERROR'
# exit(1)
position_memory = Position_Memory()
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('drone01_server', sm, '/SM_DRONE01')
sis.start()
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('MISSION', CBState(loadMission_cb),
transitions={'finished': 'TAKEOFF', 'failed':'LAND'})
smach.StateMachine.add('TAKEOFF', CBState(takeoff_cb),
transitions={'finished': 'MISSION_SEARCHING', 'failed':'LAND'})
smach.StateMachine.add('MISSION_SEARCHING', CBState(auto_cb),
transitions={'finished': 'RTL', 'reaching': 'MISSION_REACHING', 'failed':'RTL'},
remapping={'target_dx': 'target_dx','target_dy': 'target_dy','target_loc': 'target_loc','errX_pix': 'errX_pix','errY_pix': 'errY_pix'})
smach.StateMachine.add('MISSION_REACHING', CBState(reaching_cb),
transitions={'finished': 'MISSION_ACTION', 'GPS': 'MISSION_REACHINGgps', 'failed':'RTL'},
remapping={'target_dx': 'target_dx','target_dy': 'target_dy','target_loc': 'target_loc','errX_pix': 'errX_pix','errY_pix': 'errY_pix'})
smach.StateMachine.add('MISSION_REACHINGgps', CBState(reachingGPS_cb),
transitions={'finished': 'MISSION_SEARCHING','reaching': 'MISSION_REACHING','failed':'RTL'},
remapping={'target_loc': 'target_loc'})
smach.StateMachine.add('MISSION_ACTION', CBState(action_cb),
transitions={'finished': 'MISSION_SEARCHING', 'failed':'RTL'})
smach.StateMachine.add('RTL', CBState(RTL_cb),
return 'timeouted'
if __name__ == "__main__":
rospy.init_node('readtopicstatetest')
from smach import StateMachine, CBState
from std_msgs.msg import String
sm = StateMachine(outcomes=['succeeded'])
with sm:
StateMachine.add('READTOPIC',
ReadTopicState('/test',
String,
output_key_name='test_key',
timeout=10),
transitions={
'succeeded': 'succeeded',
'timeouted': 'SAYTIMEOUT'
})
def cb(ud):
rospy.logwarn(
'READ TOPIC STATE TIMEOUTED WITHOUT RECEIVING MESSAGES')
return 'succeeded'
StateMachine.add('SAYTIMEOUT',
CBState(cb, outcomes=['succeeded']),
transitions={'succeeded': 'READTOPIC'})
sm.execute()
rospy.loginfo('Data received is: ' + sm.userdata.test_key.data)
return 'finished'
#else:
#return 'failed'
if __name__ == '__main__':
rospy.init_node('drone_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome'])
# Open the container
with sm:
# Stand UP state
smach.StateMachine.add('STAND_UP', CBState(stand_up_cb), {
'finished': 'STOP',
'failed': 'outcome'
})
# Stop state
smach.StateMachine.add(
'STOP', CBState(stop_cb), {
'finished_1': 'ALTERN_TRIPOD',
'finished_2': 'TETRAPOD',
'finished_3': 'STAIR_CLIMB',
'failed': 'outcome'
})
# Altern tripod state
smach.StateMachine.add('ALTERN_TRIPOD', CBState(altern_tripod_cb), {
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, heritage):
# Creation of State Machine and definition of its outcomes
self.agent_sm = StateMachine(outcomes=['completed', 'failed'])
with self.agent_sm:
# Initialization of the dictionary containing every Action Service Wrapper
self.asw_dicc = {}
### ACTION SERVER ADVERTISING ###
# Add a state where drone does nothing but wait to receive a service order
StateMachine.add(
'action_server_advertiser',
CBState(self.action_server_advertiser_stcb,
cb_kwargs={
'heritage': heritage,
'asw_dicc': self.asw_dicc
}), {'completed': 'completed'})
# Every action service wrapper is similar following a structure defined by SMACH.
# Each wrapper is defined and added to
# To understand what they do, see callbacks below
### TAKE-OFF STATE MACHINE & WRAPPER ###
self.take_off_sm = StateMachine(outcomes=['completed', 'failed'],
input_keys=['action_goal'])
self.asw_dicc['take_off'] = ActionServerWrapper(
'/magna/GS_Agent_{}/take_off_command'.format(heritage.ID),
TakeOffAction,
self.take_off_sm, ['completed'], ['failed'], ['preempted'],
goal_key='action_goal',
result_key='action_result')
with self.take_off_sm:
StateMachine.add(
'take_off',
CBState(self.take_off_stcb,
input_keys=['action_goal'],
cb_kwargs={'heritage': heritage}),
{'completed': 'completed'})
### LAND STATE MACHINE & WRAPPER ###
self.land_sm = StateMachine(outcomes=['completed', 'failed'],
input_keys=['action_goal'])
self.asw_dicc['land'] = ActionServerWrapper(
'/magna/GS_Agent_{}/land_command'.format(heritage.ID),
LandAction,
self.land_sm, ['completed'], ['failed'], ['preempted'],
goal_key='action_goal',
result_key='action_result')
with self.land_sm:
StateMachine.add(
'land',
CBState(self.land_stcb,
input_keys=['action_goal'],
cb_kwargs={'heritage': heritage}),
{'completed': 'completed'})
### BASIC MOVE STATE MACHINE & WRAPPER ###
self.basic_move_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['basic_move'] = ActionServerWrapper(
'/magna/GS_Agent_{}/basic_move_command'.format(heritage.ID),
BasicMoveAction,
self.basic_move_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.basic_move_sm:
StateMachine.add(
'basic_move',
CBState(self.basic_move_stcb,
input_keys=['action_goal', 'action_result'],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
### SET MISSION STATE MACHINE & WRAPPER ###
self.set_mission_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['set_mission'] = ActionServerWrapper(
'/magna/GS_Agent_{}/set_mission_command'.format(heritage.ID),
SetMissionAction,
self.set_mission_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.set_mission_sm:
StateMachine.add(
'set_mission',
CBState(self.set_mission_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
### FOLLOW PATH STATE MACHINE & WRAPPER ###
self.follow_path_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['follow_path'] = ActionServerWrapper(
'/magna/GS_Agent_{}/follow_path_command'.format(heritage.ID),
FollowPathAction,
self.follow_path_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.follow_path_sm:
StateMachine.add(
'follow_path',
CBState(self.follow_path_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
# StateMachine.add('to_wp', self.follow_path_sm,
# {'completed':'action_server_advertiser'})
### FOLLOW Agent AD STATE MACHINE & WRAPPER###
self.follow_agent_ad_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['follow_agent_ad'] = ActionServerWrapper(
'/magna/GS_Agent_{}/follow_agent_ad_command'.format(
heritage.ID),
FollowAgentADAction,
self.follow_agent_ad_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.follow_agent_ad_sm:
StateMachine.add(
'follow_agent_ad',
CBState(self.follow_agent_ad_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
### FOLLOW Agent AP STATE MACHINE & WRAPPER###
self.follow_agent_ap_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['follow_agent_ap'] = ActionServerWrapper(
'/magna/GS_Agent_{}/follow_agent_ap_command'.format(
heritage.ID),
FollowAgentAPAction,
self.follow_agent_ap_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.follow_agent_ap_sm:
StateMachine.add(
'follow_agent_ap',
CBState(self.follow_agent_ap_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
if heritage.smach_view == True:
sis = smach_ros.IntrospectionServer(
'magna/Agent_{}_introspection'.format(heritage.ID),
self.agent_sm, '/Agent_{}'.format(heritage.ID))
sis.start()
def main():
# The autonomy_sm handles the full autonomy sequence for the competition run
autonomy_sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with autonomy_sm:
# Add states for setup (these run once each)
for i in range(len(setup_sequence)):
name, action_type = get_name_and_type(setup_sequence[i])
func_to_run = None
if action_type == 'localize':
func_to_run = localization_cb
elif action_type == 'move':
# action instead of function
func_to_run = 'action'
else:
raise NotImplementedError('Only localize is supported rn')
# if this is the last setup state
if i == len(loop_sequence) - 1:
# tie the last setup state to the first loop state
first_loop_name = get_name_and_type(loop_sequence[0])
if func_to_run == 'action':
action_state = create_action_state(name, action_type)
StateMachine.add(
name,
action_state,
transitions={'succeeded': first_loop_name})
else:
StateMachine.add(name, CBState(func_to_run),
{'succeeded': first_loop_name})
else:
# add_auto adds this to the state machine and automatically
# ties the next and previous states together
if func_to_run == 'action':
action_state = create_action_state(name, action_type)
StateMachine.add_auto(name,
action_state,
connector_outcomes=['succeeded'])
else:
StateMachine.add_auto(name,
CBState(func_to_run),
connector_outcomes=['succeeded'])
names = [] # store names to check and handle dupes
for i in range(len(loop_sequence)):
name, action_type = get_name_and_type(loop_sequence[i])
action_state = create_action_state(name, action_type)
# "move" states all have the same name and smach requires unique
# state names, so check and add a number to the name if needed
if name in names:
name = name + "2"
while name in names:
name = name[:-1] + str(
int(name[-1]) + 1) # increase num by 1
names.append(name)
if i == len(loop_sequence) - 1:
# tie the last state to the first one, so they just keep looping
StateMachine.add(name,
action_state,
transitions={'succeeded': names[0]})
else:
# add_auto adds this to the state machine and automatically
# ties the next and previous states together
StateMachine.add_auto(name,
action_state,
connector_outcomes=['succeeded'])
# Create the concurrence container for the fully autonomy sequence. This
# runs the state machine for the competition run. It also concurrently runs
# a state that listens to the /click_start_button topic. If this is
# triggered place us into the teleop state.
autonomy_concurrence = Concurrence(
outcomes=['enter_teleop', 'stay', 'aborted'],
default_outcome='enter_teleop',
child_termination_cb=autonomy_child_term_cb,
outcome_cb=autonomy_out_cb)
with autonomy_concurrence:
# state that runs full autonomy state machine
Concurrence.add('AUTONOMY', autonomy_sm)
# state that listens for toggle message
Concurrence.add(
'TOGGLE_LISTEN',
MonitorState('/click_start_button', Empty, start_btn_cb))
teleop_concurrence = Concurrence(
outcomes=['enter_autonomy', 'stay', 'done'],
default_outcome='enter_autonomy',
child_termination_cb=teleop_child_term_cb,
outcome_cb=teleop_out_cb)
with teleop_concurrence:
Concurrence.add(
'TOGGLE_LISTEN',
MonitorState('/click_start_button', Empty, start_btn_cb))
Concurrence.add(
'EXIT_LISTEN',
MonitorState('/click_select_button', Empty, select_btn_cb))
# Top level state machine, containing the autonomy and teleop machines.
top_sm = StateMachine(outcomes=['DONE'])
with top_sm:
StateMachine.add('TELEOP_MODE',
teleop_concurrence,
transitions={
'enter_autonomy': 'AUTONOMY_MODE',
'stay': 'TELEOP_MODE',
'done': 'DONE'
})
StateMachine.add('AUTONOMY_MODE',
autonomy_concurrence,
transitions={
'enter_teleop': 'TELEOP_MODE',
'stay': 'AUTONOMY_MODE',
'aborted': 'DONE'
})
#StateMachine.add('TELEOP_MODE', MonitorState('/click_start_button', Empty, monitor_cb),
# transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})
sis = IntrospectionServer('smach_introspection_server', top_sm,
'/COMPETITION_SMACH')
sis.start()
top_sm.execute()
rospy.spin()
sis.stop()
def __init__(self, robot, table_id, placement_height):
StateMachine.__init__(self,
outcomes=['succeeded', 'failed'],
input_keys=["item_picked"])
arm = robot.get_arm()._arm
self.placement_height = placement_height
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_joint_trajectory(goal_array, wait_for_motion_done=True):
arm._send_joint_trajectory(goal_array, timeout=rospy.Duration(0))
if wait_for_motion_done:
arm.wait_for_motion_done()
def send_gripper_goal(open_close_string):
arm.send_gripper_goal(open_close_string)
rospy.sleep(1.0) # Does not work with motion_done apparently
@cb_interface(outcomes=['done'], input_keys=["item_picked"])
def _pre_place(user_data):
rospy.loginfo("Preplacing...")
robot.head.look_up()
robot.head.wait_for_motion_done()
item_name = user_data["item_picked"]
send_joint_goal([0.69, 0, 0, 0, 0])
if item_name in ['plate', 'napkin']:
send_joint_goal(pboven)
else:
send_joint_goal([0.69, -1.2, 0, -1.57, 0])
return 'done'
@cb_interface(outcomes=['done'], input_keys=["item_picked"])
def _align_with_table(user_data):
item_placement_vector = item_vector_dict[user_data["item_picked"]]
item_frame = item_vector_to_item_frame(item_placement_vector)
goal_pose = item_frame_to_pose(item_frame, table_id)
rospy.loginfo("Placing {} at {}".format(user_data["item_picked"],
goal_pose))
robot.head.look_down()
robot.head.wait_for_motion_done()
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02,
0.1)).execute({})
return 'done'
@cb_interface(outcomes=['done'], input_keys=["item_picked"])
def _place_and_retract(user_data):
rospy.loginfo("Placing...")
item_name = user_data["item_picked"]
if item_name in ['plate', 'napkin']:
# TODO: Do a different joint goal/trajectory
send_joint_goal(pleg)
# send_joint_goal(pweg)
else:
send_joint_goal([self.placement_height, -1.57, 0, -1.57, 0])
rospy.loginfo("Dropping...")
send_gripper_goal("open")
send_joint_goal(pweg)
robot.head.look_up()
robot.head.wait_for_motion_done()
rospy.loginfo("Retract...")
send_joint_goal([0.69, 0, -1.57, 0, 0])
send_gripper_goal("close")
robot.base.force_drive(
-0.1, 0, 0, 3) # Drive backwards at 0.1m/s for 3s, so 30cm
arm.send_joint_goal("carrying_pose")
return 'done'
@cb_interface(outcomes=['done'], input_keys=["item_picked"])
def _ask_user(user_data):
robot.head.look_up()
robot.head.wait_for_motion_done()
send_joint_goal([0, 0, 0, 0, 0])
item_name = user_data["item_picked"]
robot.speech.speak(
"Please put the {} on the table.".format(item_name))
robot.speech.speak("Watch out, I will open my gripper now")
send_gripper_goal("open")
rospy.sleep(5.0)
robot.speech.speak("Thanks for that!", block=False)
send_gripper_goal("open")
arm.send_joint_goal("carrying_pose", timeout=0)
return 'done'
with self:
self.add_auto('PRE_PLACE', CBState(_pre_place), ['done'])
self.add_auto('ALIGN_WITH_TABLE', CBState(_align_with_table),
['done'])
self.add('PLACE_AND_RETRACT',
CBState(_place_and_retract),
transitions={'done': 'succeeded'})
def get_smach_state(self):
return CBState(takeoff_cb)
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'})
sm_one = smach.StateMachine(outcomes=[
'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED',
'ACTION_ERROR'
])
with sm_one:
smach.StateMachine.add(
'CONDITION_30',
smach_ros.MonitorState('/ros_cle_simulation/status', String,
condition_30_cb), {
'valid': 'CONDITION_30',
'invalid': 'ACTION_30',
'preempted': 'CONDITION_PREEMPTED'
})
smach.StateMachine.add('ACTION_30', CBState(action_30_cb),
{'finished': 'FINISHED'})
sm_two = smach.StateMachine(outcomes=[
'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED',
'ACTION_ERROR'
])
with sm_two:
smach.StateMachine.add(
'CONDITION_SPIKE',
smach_ros.MonitorState(
'/monitoring/left_wheel_neuron_rate_monitor', String,
condition_spike_cb), {
'valid': 'CONDITION_SPIKE',
'invalid': 'ACTION_SPIKE',
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):
# Create a SMACH state machine
self.sm = smach.StateMachine(outcomes=['succeeded', 'aborted'])
with self.sm:
# Add states to the state machine
smach.StateMachine.add('GO_TO_CENTER',
CBState(self.go_to_center, cb_args=[self]),
transitions={
'lock': 'GO_TO_CENTER',
'succeeded': 'POSITION_CONTROL',
'aborted': 'aborted'
})
smach.StateMachine.add('POSITION_CONTROL',
CBState(self.position_control,
cb_args=[self]),
transitions={
'stay': 'POSITION_CONTROL',
'leave': 'VIBRATORY_PHASE',
'aborted': 'aborted'
})
smach.StateMachine.add('VIBRATORY_PHASE',
CBState(self.vibratory_phase,
cb_args=[self]),
transitions={
'vibrate': 'VIBRATORY_PHASE',
'succeeded': 'RATE_CONTROL',
'aborted': 'aborted'
})
smach.StateMachine.add('RATE_CONTROL',
CBState(self.rate_control, cb_args=[self]),
transitions={
'stay': 'RATE_CONTROL',
'leave': 'GO_TO_CENTER',
'collision': 'RATE_COLLISION',
'aborted': 'aborted'
})
smach.StateMachine.add('RATE_COLLISION',
CBState(self.rate_collision,
cb_args=[self]),
transitions={
'succeeded': 'GO_TO_CENTER',
'aborted': 'aborted'
})
# Read all the parameters from the parameter server
# Topics to interact
master_name = self.read_parameter('~master_name', 'phantom')
slave_name = self.read_parameter('~slave_name', 'grips')
self.master_state_topic = '/%s/state' % master_name
self.feedback_topic = '/%s/force_feedback' % master_name
self.slave_state_topic = '/%s/state' % slave_name
self.ik_mc_topic = '/%s/ik_command' % slave_name
self.gripper_topic = '/%s/GRIP/command' % slave_name
# Workspace definition
self.units = self.read_parameter('~units', 'mm')
width = self.read_parameter('~workspace/width', 140.0)
height = self.read_parameter('~workspace/height', 100.0)
depth = self.read_parameter('~workspace/depth', 55.0)
self.center_pos = self.read_parameter('~workspace/center', [0, 0, 0])
self.workspace = np.array([width, depth, height])
self.hysteresis = self.read_parameter('~hysteresis', 3.0)
self.pivot_dist = self.read_parameter('~pivot_dist', 5.0)
# Force feedback parameters
self.k_center = self.read_parameter('~k_center', 0.1)
self.b_center = self.read_parameter('~b_center', 0.003)
self.k_rate = self.read_parameter('~k_rate', 0.05)
self.b_rate = self.read_parameter('~b_rate', 0.003)
# Position parameters
self.rpy_offset = self.read_parameter('~rpy_offset', [0, 0, 0])
self.q_offset = tr.quaternion_from_euler(*self.rpy_offset)
self.publish_frequency = self.read_parameter('~publish_rate', 1000.0)
self.position_ratio = self.read_parameter('~position_ratio', 250)
# Vibration parameters
self.vib_a = self.read_parameter('~vibration/a', 2.0) # Amplitude (mm)
self.vib_c = self.read_parameter('~vibration/c', 5.0) # Damping
self.vib_freq = self.read_parameter('~vibration/frequency',
30.0) # Frequency (Hz)
self.vib_time = self.read_parameter('~vibration/duration',
0.3) # Duration (s)
self.vib_start_time = 0.0
# Rate parameters
self.rate_pivot = np.zeros(3)
self.rate_gain = self.read_parameter('~rate_gain', 1.0)
# Initial values
self.frame_id = self.read_parameter('~reference_frame', 'world')
self.colors = TextColors()
self.gripper_cmd = 0.0
self.master_pos = None
self.master_rot = np.array([0, 0, 0, 1])
self.master_vel = np.zeros(3)
self.master_dir = np.zeros(3)
self.slave_pos = None
self.slave_rot = np.array([0, 0, 0, 1])
self.timer = None
self.force_feedback = np.zeros(3)
# Synch
self.slave_synch_pos = np.zeros(3)
self.slave_synch_rot = np.array([0, 0, 0, 1])
self.master_synch_rot = np.array([0, 0, 0, 1])
# Setup Subscribers/Publishers
self.feedback_pub = rospy.Publisher(self.feedback_topic, OmniFeedback)
self.ik_mc_pub = rospy.Publisher(self.ik_mc_topic, PoseStamped)
self.gripper_pub = rospy.Publisher(self.gripper_topic, Float64)
self.vis_pub = rospy.Publisher('visualization_marker', Marker)
rospy.Subscriber(self.master_state_topic, OmniState,
self.cb_master_state)
rospy.Subscriber(self.slave_state_topic, EndpointState,
self.cb_slave_state)
self.loginfo('Waiting for [%s] and [%s] topics' %
(self.master_state_topic, self.slave_state_topic))
while not rospy.is_shutdown():
if (self.slave_pos == None) or (self.master_pos == None):
rospy.sleep(0.01)
else:
self.loginfo('Rate position controller running')
# Register rospy shutdown hook
rospy.on_shutdown(self.shutdown_hook)
break
# Make sure the first command sent to the slave is equal to its current position6D
self.command_pos = np.array(self.slave_pos)
self.command_rot = np.array(self.slave_rot)
# Start the timer that will publish the ik commands
self.timer = rospy.Timer(rospy.Duration(1.0 / self.publish_frequency),
self.publish_command)
self.loginfo('State machine state: GO_TO_CENTER')
def construct_sm():
sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
sm.userdata.nums = range(25, 88, 3)
sm.userdata.even_nums = []
sm.userdata.odd_nums = []
with sm:
## %Tag(ITERATOR)%
tutorial_it = Iterator(
outcomes=["succeeded", "preempted", "aborted"],
input_keys=["nums", "even_nums", "odd_nums"],
it=lambda: range(0, len(sm.userdata.nums)),
output_keys=["even_nums", "odd_nums"],
it_label="index",
exhausted_outcome="succeeded",
)
## %EndTag(ITERATOR)%
## %Tag(CONTAINER)%
with tutorial_it:
container_sm = StateMachine(
outcomes=["succeeded", "preempted", "aborted", "continue"],
input_keys=["nums", "index", "even_nums", "odd_nums"],
output_keys=["even_nums", "odd_nums"],
)
with container_sm:
# test wether even or odd
StateMachine.add(
"EVEN_OR_ODD",
ConditionState(
cond_cb=lambda ud: ud.nums[ud.index] % 2,
input_keys=["nums", "index"],
),
{
"true": "ODD",
"false": "EVEN"
},
)
# add even state
@smach.cb_interface(
input_keys=["nums", "index", "even_nums"],
output_keys=["odd_nums"],
outcomes=["succeeded"],
)
def even_cb(ud):
ud.even_nums.append(ud.nums[ud.index])
return "succeeded"
StateMachine.add("EVEN", CBState(even_cb),
{"succeeded": "continue"})
# add odd state
@smach.cb_interface(
input_keys=["nums", "index", "odd_nums"],
output_keys=["odd_nums"],
outcomes=["succeeded"],
)
def odd_cb(ud):
ud.odd_nums.append(ud.nums[ud.index])
return "succeeded"
StateMachine.add("ODD", CBState(odd_cb),
{"succeeded": "continue"})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
# close container_sm
Iterator.set_contained_state("CONTAINER_STATE",
container_sm,
loop_outcomes=["continue"])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
# close the tutorial_it
StateMachine.add("TUTORIAL_IT", tutorial_it, {
"succeeded": "succeeded",
"aborted": "aborted"
})
## %EndTag(ADDITERATOR)%
return sm
def __init__(self):
rospy.init_node('Shopping')
self.smach_bool = False
rospy.on_shutdown(self.shutdown)
self.sm_EnterRoom = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.sm_EnterRoom:
StateMachine.add('DOOR_DETECT',
DoorDetect().door_detect_,
transitions={
'invalid': 'WAIT',
'valid': 'DOOR_DETECT',
'preempted': 'aborted'
})
# waits
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'ENTER',
'error': 'error'
},
remapping={'rec': 'wait_len'})
# StateMachine.add('ENTER', LinearDisplacement(), transitions={'succeeded': 'succeeded',
# 'preempted': 'ENTER',
# 'error': 'error'},
# remapping={'displacementVec': 'point'})
self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
'pos']
StateMachine.add('ENTER',
NavStack(),
transitions={
'succeeded': 'succeeded',
'aborted': 'ENTER',
'error': 'error'
},
remapping={'pos_xm': 'start_waypoint'})
# how to get stop signal?
self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'],
default_outcome='stop',
outcome_map={
'remeber': {
'STOP': 'remeber'
},
'stop': {
'STOP': 'stop'
},
'aborted': {
'FOLLOW': 'aborted'
}
},
child_termination_cb=self.trace_child_cb,
input_keys=['PT_LIST', 'mission'],
output_keys=['PT_LIST', 'mission'])
with self.trace:
self.meta_follow = StateMachine(
['succeeded', 'aborted', 'preempted'])
with self.meta_follow:
self.meta_follow.userdata.pos_xm = Pose()
StateMachine.add('FIND',
FindPeople().find_people_,
transitions={
'invalid': 'META_NAV',
'valid': 'FIND',
'preempted': 'preempted'
},
remapping={'pos_xm': 'pos_xm'})
self.meta_nav = Concurrence(
outcomes=['time_over', 'get_pos', 'aborted'],
default_outcome='aborted',
outcome_map={
'time_over': {
'WAIT': 'succeeded'
},
'get_pos': {
'NAV': 'succeeded'
},
'aborted': {
'NAV': 'aborted'
}
},
child_termination_cb=self.nav_child_cb,
input_keys=['pos_xm'])
with self.meta_nav:
Concurrence.add('NAV',
NavStack(),
remapping={'pos_xm': 'pos_xm'})
Concurrence.add('WAIT', Wait_trace())
StateMachine.add('META_NAV',
self.meta_nav,
transitions={
'get_pos': 'FIND',
'time_over': 'FIND',
'aborted': 'FIND'
})
Concurrence.add('FOLLOW', self.meta_follow)
Concurrence.add('STOP',
CheckStop2(),
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
self.Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['target'])
with self.Pick:
self.Pick.userdata.target_pos = PointStamped()
self.Pick.userdata.nav_pos = Pose()
self.Pick.userdata.pick_pos = PointStamped()
self.Pick.userdata.distance = 1.0
self.Pick.userdata.distance2 = 0.9
self.Pick.userdata.target_mode = 1
self.Pick.userdata.objmode = 1
#self.Pick.userdata.target = 'ice_tea'
StateMachine.add('FIND',
FindObject(),
transitions={
'succeeded': 'DISTANCE',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'name': 'target',
'object_pos': 'target_pos',
'object_map_point': 'object_map_point'
})
# StateMachine.add('FIND', self.FindObj,
# transitions={'succeeded': 'DISTANCE',
# 'aborted': 'aborted', 'error': 'error'},
# remapping={'target': 'target',
# 'indice': 'indice',
# 'targets': 'targets',
# 'object_map_point':'object_map_point',
# 'target_pos': 'target_pos'})
# StateMachine.add('FIND', self.FindObj,
# transitions={'succeeded': 'DISTANCE',
# 'aborted': 'aborted', 'error': 'error'},
# remapping = {'name':'target' ,
# 'object_pos':'target_pos',
# 'object_map_point':'object_map_point'})
StateMachine.add('DISTANCE',
CBState(self.PickDistance,
outcomes=['succeeded', 'error'],
input_keys=['name'],
output_keys=['distance']),
transitions={
'succeeded': 'POS_JUS',
'error': 'POS_JUS'
},
remapping={
'distance': 'distance',
'name': 'target'
})
StateMachine.add('POS_JUS',
new_PosJustfy(),
transitions={
'succeeded': 'NAV',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'pose': 'nav_pos',
'distance': 'distance',
'object_pos': 'target_pos'
})
StateMachine.add('NAV',
NavStack(),
transitions={
'succeeded': 'FIND_AGAIN',
'aborted': 'NAV',
'error': 'error',
},
remapping={'pos_xm': 'nav_pos'})
StateMachine.add('PICK2',
ArmCmdForTf(),
transitions={
'succeeded': 'succeeded',
'error': 'error',
'aborted': 'aborted'
},
remapping={
'arm_ps': 'object_map_point',
'mode': 'objmode'
})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK',
'aborted': 'PICK2',
'error': 'error'
},
remapping={
'object_pos': 'pick_pos',
'name': 'target',
'object_state': 'object_state'
})
StateMachine.add('PICK',
new_ArmCmd(),
transitions={
'succeeded': 'succeeded',
'error': 'error',
'aborted': 'aborted'
},
remapping={
'arm_ps': 'pick_pos',
'mode': 'objmode',
'object_state': 'object_state'
})
self.sm_FaceDetect = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
output_keys=['people_position', 'num_list'])
with self.sm_FaceDetect:
self.sm_FaceDetect.userdata.people_position = list()
self.sm_FaceDetect.userdata.sentences = 'please look at me'
StateMachine.add('SPEAK',
Speak(),
remapping={'sentences': "sentences"},
transitions={
'succeeded': 'GET_POSITION',
'aborted': 'aborted',
'error': 'error'
})
# call face_reco service for get all the people position which is a list
self.sm_FaceDetect.userdata.name_id = -1 #
self.sm_FaceDetect.userdata.num_list = list()
self.sm_FaceDetect.userdata.distance = 0.6
StateMachine.add('GET_POSITION',
FaceReco(),
remapping={
'name_id': 'name_id',
'position': 'people_position',
'num_list': 'num_list',
'distance': 'distance'
},
transitions={
'succeeded': 'succeeded',
'again': 'GET_POSITION',
'aborted': 'GET_POSITION',
'error': 'error',
'turn_l': 'MOVEAHEAD',
'turn_r': 'MOVEAHEAD',
'train_error': 'aborted'
})
self.sm_FaceDetect.userdata.point_1 = Point(0.1, 0.0, 0.0)
StateMachine.add(
'MOVEAHEAD',
SimpleMove_move(),
transitions={
'succeeded': 'GET_POSITION',
'error': 'error',
'aborted': 'GET_POSITION'
},
remapping={'point': 'point_1'},
)
self.GetTask = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
output_keys=['task'])
with self.GetTask:
self.GetTask.userdata.people_position = list()
self.GetTask.userdata.num_list = list()
self.GetTask.userdata.person_position = Pose()
StateMachine.add('GET_POSITION',
self.sm_FaceDetect,
transitions={
'succeeded': 'GET_VALUE1',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'people_position': 'people_position',
'num_list': 'num_list'
})
StateMachine.add('GET_VALUE1',
GetValue(),
remapping={
'element_list': 'people_position',
'element': 'person_position'
},
transitions={
'succeeded': 'NAV1',
'aborted': "succeeded",
'error': 'error'
})
StateMachine.add('NAV1',
NavStack(),
transitions={
'succeeded': 'ASK_TASK1',
'aborted': 'NAV1',
'error': 'error'
},
remapping={'pos_xm': 'person_position'})
self.GetTask.userdata.sentence1 = 'what do you want?'
StateMachine.add('ASK_TASK1',
Speak(),
transitions={
'succeeded': 'ANS1',
'aborted': 'GET_VALUE1',
'error': 'error'
},
remapping={'sentence': 'sentence1'})
StateMachine.add('ANS1',
ShoppingGetTask(),
transitions={
'succeeded': 'GET_VALUE1',
'aborted': 'GET_VALUE1'
},
remapping={'task': 'task'})
# StateMachine.add('ASK_TASK2' , Speak() , transitions={'succeeded':'ANS2',
# 'aborted':'aborted',
# 'error':'error'},
# remapping={'sentence':'sentence1'})
# StateMachine.add('ANS2' , ShoppingGetTask() , transitions={'succeeded':'succeeded',
# 'aborted':'aborted'},
# remapping={'task':'task'})
self.DealTask = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['task', 'mission'])
with self.DealTask:
self.DealTask.userdata.nav_pos = Pose()
self.DealTask.userdata.indice = 0
self.DealTask.userdata.indice2 = 3
self.DealTask.userdata.name = ''
StateMachine.add('NXT_TASK',
ShoppingNextTask(),
transitions={
'go': 'NAV',
'back': 'NAV_CASH',
'finish': 'succeeded',
'aborted': "aborted"
},
remapping={
'pose': 'nav_pos',
'name': 'name',
'indice': 'indice'
})
StateMachine.add('NAV',
NavStack(),
transitions={
'succeeded': 'PICK',
'aborted': 'NAV',
'error': 'error'
},
remapping={'pos_xm', 'nav_pos'})
StateMachine.add('PICK',
self.Pick,
transitions={
'succeeded': 'NXT_TASK',
'aborted': 'NXT_TASK',
'error': 'error'
},
remapping={'target': 'name'})
StateMachine.add('NAV_CASH',
NavStack(),
transitions={
'succeeded': 'PLACE',
'aborted': 'NAV_CASH',
'error': 'error'
},
remapping={'pos_xm', 'nav_pos'})
#place need to be upgraded
StateMachine.add('PLACE',
Place2(),
transitions={
'succeeded': 'NXT_TASK',
'aborted': 'NXT_TASK',
})
self.shopping = StateMachine(
outcomes=['succeeded', 'aborted', 'error'])
with self.shopping:
self.shopping.userdata.PT_LIST = {}
self.shopping.userdata.mission = {}
self.shopping.userdata.task = list()
self.shopping.userdata.rec = 5.0
# StateMachine.add('ENTERROOM',
# self.sm_EnterRoom,
# transitions={'succeeded':'START','aborted':'aborted'})
StateMachine.add('START',
GetSignal(),
transitions={
'succeeded': 'RUNNODE',
'aborted': 'aborted'
})
StateMachine.add('RUNNODE',
RunNode(),
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'TRACE',
'error': 'error'
},
remapping={'rec': 'rec'})
StateMachine.add('TRACE',
self.trace,
transitions={
'remeber': 'TRACE',
'stop': 'GET_TASK',
'aborted': 'aborted'
},
remapping={
'PT_LIST': 'PT_LIST',
'mission': 'mission'
})
StateMachine.add('GET_TASK',
self.GetTask,
transitions={
'succeeded': 'DEAL_TASK',
'aborted': 'aborted',
'error': 'error'
},
remapping={'task': 'task'})
StateMachine.add('DEAL_TASK',
self.DealTask,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={'task': 'task'})
intro_server = IntrospectionServer('shopping', self.shopping,
'SM_ROOT')
intro_server.start()
out = self.shopping.execute()
intro_server.stop()
if out == 'succeeded':
self.smach_bool = True
def __init__(self, robot, furniture_designator):
# type: (Hero, VariableDesignator) -> None
StateMachine.__init__(self,
outcomes=['done'],
output_keys=["laser_dot"])
is_writeable(furniture_designator)
def _show_view(timeout=5):
rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo()
robot.hmi.show_image_from_msg(rgb, timeout)
return rgb, depth, depth_info
@cb_interface(outcomes=['done'])
def _prepare_operator(_):
global OPERATOR
OPERATOR = None
robot.ed.reset()
robot.head.reset()
robot.speech.speak("Let's point, please stand in front of me!",
block=False)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
robot.speech.speak(
"Please point at the object you want me to hand you",
block=False) # hmm, weird sentence
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=2)
rospy.sleep(0.4)
_show_view(timeout=1)
robot.speech.speak("Three")
_show_view(timeout=1)
robot.speech.speak("Two")
_show_view(timeout=1)
robot.speech.speak("One")
return 'done'
@cb_interface(outcomes=['done'])
def _get_operator(_):
global OPERATOR
def _is_operator(person):
if person.position.z > 2.5:
robot.speech.speak(
"Please stand in my view with your full body")
return False
if person.position.z < 1.5:
robot.speech.speak(
"Please stand in my view with your full body")
return False
if "is_pointing" not in person.tags:
robot.speech.speak("Please point with your arm stretched")
return False
return True
while not rospy.is_shutdown() and OPERATOR is None:
persons = robot.perception.detect_person_3d(*_show_view())
if persons:
persons = sorted(persons, key=lambda x: x.position.z)
person = persons[0]
if _is_operator(person):
OPERATOR = person
# robot.speech.speak("I see an operator at %.2f meter in front of me" % OPERATOR.position.z)
rospy.loginfo("I see an operator at %.2f meter in front of me" %
OPERATOR.position.z)
return 'done'
@cb_interface(outcomes=['done', 'failed'], output_keys=['laser_dot'])
def _get_furniture(user_data):
global OPERATOR
final_result = None
while not rospy.is_shutdown() and final_result is None:
result = None
while not rospy.is_shutdown() and result is None:
try:
map_pose = robot.tf_listener.transformPose(
"map",
PoseStamped(header=Header(
frame_id=OPERATOR.header.frame_id,
stamp=rospy.Time.now() -
rospy.Duration.from_sec(0.5)),
pose=OPERATOR.pointing_pose))
result = robot.ed.ray_trace(map_pose)
except Exception as e:
rospy.logerr(
"Could not get ray trace from closest person: {}".
format(e))
rospy.sleep(1.)
# result.intersection_point type: PointStamped
# result.entity_id: string
rospy.loginfo(
"There is a ray intersection with {i} at ({p.x:.4}, {p.y:.4}, {p.z:.4})"
.format(i=result.entity_id,
p=result.intersection_point.point))
if result.entity_id in all_possible_furniture:
final_result = result
else:
rospy.loginfo("{} is not furniture".format(
result.entity_id))
robot.speech.speak("That's not furniture, you dummy.")
rospy.sleep(3)
OPERATOR = None
robot.get_arm().send_joint_goal("reset")
return 'failed'
robot.speech.speak("You pointed at %s" % final_result.entity_id)
robot.get_arm().send_joint_goal("reset")
# Fill the designator and user data the furniture inspection
furniture_designator.write(
robot.ed.get_entity(final_result.entity_id))
user_data['laser_dot'] = result.intersection_point
return 'done'
with self:
self.add('PREPARE_OPERATOR',
CBState(_prepare_operator),
transitions={'done': 'GET_OPERATOR'})
self.add('GET_OPERATOR',
CBState(_get_operator),
transitions={'done': 'GET_FURNITURE'})
self.add('GET_FURNITURE',
CBState(_get_furniture),
transitions={
'done': 'done',
'failed': 'GET_OPERATOR'
})
def __init__(self):
rospy.init_node('whoiswho_nopick')
rospy.logwarn('start who is who test')
self.smach_bool =False
rospy.on_shutdown(self.shutdown)
# subprocess.call("xterm -e rosrun xm_speech xm_speech_client.py &", shell = True)
# add waypoints into the list
self.waypoints=[]
location= (Point(1.003, -0.217, 0.000), #找人数的点
Point( 5.284, -1.246, 0.000), #出门抓东西的点
Point( 0.000,0.000,0.000)) #结束出门的点
quaternions=(Quaternion(0.000, 0.000, 0.723, 0.691),
Quaternion(0.000, 0.000,0.050, 0.999),
Quaternion(0.000,0.000,0.000, 1))
# euler_angles=[0.000,3.1415,0.000]
# for angle in euler_angles:
# q_angle = quaternion_from_euler(0, 0, angle, axes="sxyz")
# q = Quaternion(*q_angle)
# quaternions.append(q)
for i in range(3):
self.waypoints.append(Pose(location[i], quaternions[i]))
# the locate can specified by ourselves
self.sm_EnterRoom = StateMachine(outcomes = ['succeeded','aborted','error'])
with self.sm_EnterRoom:
# rospy.logerr('sm_EnterRoom add State')
# arm go to the nav_pose in the srdf
# self.sm_EnterRoom.userdata.arm_mode =0
# self.sm_EnterRoom.userdata.arm_ps_1 = PointStamped()
# StateMachine.add('NAV_POSE',
# ArmCmd(),
# transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
# remapping ={'mode':'arm_mode','arm_ps':'arm_ps_1'})
# wait the door to open
# StateMachine.add('DOOR_DETECT',
# DoorDetect().door_detect_,
# transitions={'invalid':'WAIT','valid':'DOOR_DETECT','preempted':'error'})
# simple delay 5s
# self.sm_EnterRoom.userdata.rec = 5.0
# StateMachine.add('WAIT',
# Wait(),
# transitions={'succeeded':'NAV_1','error':'error'},
# remapping ={'rec':'rec'})
# self.sm_EnterRoom.userdata.point = Point(1.5,0.0,0.0)
# StateMachine.add('SIMPLE_MOVE',
# SimpleMove_move(),
# transitions={'succeeded':'NAV_1','aborted':'NAV_1','error':'error'},
# remapping={'point':'point'})
# navstack to room of crowds
# waypoints are the list of Pose fit the data type need
self.sm_EnterRoom.userdata.start_waypoint = self.waypoints[0]
# self.sm_EnterRoom.userdata.nav_mode =0
StateMachine.add('NAV_1',
NavStack(),
transitions={'succeeded':'SELF_INTRO','aborted':'NAV_1','error':'error'},
remapping = {'pos_xm':'start_waypoint'})
# self-introduce
self.sm_EnterRoom.userdata.sentences_2 = 'I am robot xiaomeng'
StateMachine.add('SELF_INTRO',
Speak(),
remapping ={'sentences':'sentences_2'},
transitions ={'succeeded':'succeeded','aborted':'SELF_INTRO','error':'error'})
# we have already identity the people from 0-4 when the first recongization
self.sm_FaceDetect = StateMachine(outcomes = ['succeeded','aborted','error'],
output_keys = ['people_position','num_list'])
with self.sm_FaceDetect:
self.sm_FaceDetect.userdata.people_position =list()
self.sm_FaceDetect.userdata.sentences = 'please look at me'
StateMachine.add('SPEAK',
Speak(),
remapping = {'sentences':"sentences"},
transitions = {'succeeded':'GET_POSITION','aborted':'aborted','error':'error'})
# call face_reco service for get all the people position which is a list
self.sm_FaceDetect.userdata.name_id =-1
self.sm_FaceDetect.userdata.num_list = list()
StateMachine.add('GET_POSITION',
FaceReco(),
remapping ={'name_id':'name_id','position':'people_position','num_list':'num_list'},
transitions ={'succeeded':'succeeded',
'again':'GET_POSITION',
'aborted':'GET_POSITION',
'error':'error',
'turn_l':'TURN_L',
'turn_r':'TURN_R',
'train_error':'aborted'})
# if the face-recognize failed, we should make some remedy to make the state continue
self.sm_FaceDetect.userdata.turn_point_1 = Point(0.0,0.0,pi/8)
StateMachine.add('TURN_L',
SimpleMove_move(),
transitions={'succeeded':'SPEAK_2','error':'error'},
remapping ={'point':'turn_point_1'})
self.sm_FaceDetect.userdata.turn_point_2 = Point(0.0,0.0,-pi/8)
StateMachine.add('TURN_R',
SimpleMove_move(),
remapping ={'point':'turn_point_2'},
transitions ={'succeeded':'SPEAK_2','error':'error'})
StateMachine.add('SPEAK_2',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'GET_POSITION','aborted':'aborted','error':'error'})
self.sm_GetTarget = StateMachine(outcomes =['succeeded','aborted','error'],
input_keys =['target'])#the target is a string.....
with self.sm_GetTarget:
# because xm is nav to pose in the nav_pose
self.sm_GetTarget.userdata.nav_ps = self.waypoints[1]
# this smach code donnot to grasp ,so this part is useless
StateMachine.add('NAV_TARGET',
NavStack(),
remapping ={'pos_xm':'nav_ps'},
transitions ={'succeeded':'FIND_OBJECT','aborted':'NAV_TARGET','error':'error'})
self.sm_GetTarget.userdata.object_pos = PointStamped()
StateMachine.add('FIND_OBJECT',
new_vision.FindObject(),
remapping ={'name':'target','object_pos':'object_pos'},
transitions ={'succeeded':'TALK','aborted':'FIND_OBJECT','error':'error'})
self.sm_GetTarget.userdata.sentences = 'I find the target'
StateMachine.add('TALK',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'PICK','aborted':'TALK','error':'error'})
self.sm_GetTarget.userdata.arm_mode_1 =1
StateMachine.add('PICK',
ArmCmd(),
remapping ={'mode':'arm_mode_1','arm_ps':'object_pos'},
transitions ={'succeeded':'succeeded','aborted':'succeeded','error':'error'})
self.sm_GetTarget.userdata.arm_mode_2 = 0
self.sm_GetTarget.userdata.arm_ps_2 = PointStamped()
StateMachine.add('PUT',
PlaceBag(),
transitions ={'succeeded':'succeeded','aborted':'aborted'})
# concurrence for the speech_node
# we want to make the speech-meaning a timeout, so we use the concurrence function
# but in fact we can also use multithread to slove the problem
self.meta_Remember = Concurrence(outcomes=['succeeded','aborted','error'],
output_keys=['name','target'],
default_outcome ='succeeded',
outcome_map={'succeeded':{'NAME_AND_THING':'succeeded'}},
child_termination_cb =self.child_cb)
with self.meta_Remember:
Concurrence.add('GET_EMPTY_NAME',
Hehe()
)
Concurrence.add('NAME_AND_THING',
NameAndThing(),
remapping ={'name':'name','target':'target'}
)
# input one position is a list
# io_keys can wirte and read
# userdata can update real-time
# no need to call the cv service
self.sm_Remember = StateMachine(outcomes =['succeeded','aborted','error'],
input_keys =['person_position'],
output_keys =['name','target']
)
with self.sm_Remember:
# rospy.logerr("sm_Remember add state")
# self.sm_Remember.userdata.nav_mode =1
# rospy.logerr("Is here1?")
StateMachine.add('NAV_GO',
NavStack(),
remapping ={'pos_xm':'person_position'},
transitions ={'succeeded':'TALK','aborted':'NAV_GO','error':'error'}
)
self.sm_Remember.userdata.sentences = "what is your name and what do you want"
StateMachine.add('TALK',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'GET_BOTH','aborted':'TALK','error':'error'})
# StateMachine.add('RUNNODE',
# RunNode(),
# transitions ={'succeeded':'GET_BOTH','aborted':'aborted'})
StateMachine.add('GET_BOTH',
self.meta_Remember,
remapping ={'name':'name','target':'target'},
transitions ={'succeeded':'WAIT','aborted':'GET_BOTH','error':'error'})
self.sm_Remember.userdata.rec =2.0
StateMachine.add('WAIT',
Wait(),
remapping={'rec':'rec'},
transitions ={'succeeded':'succeeded','error':"error"})
self.sm_GiveBack = StateMachine(outcomes =['succeeded','aborted','error'],
input_keys =['name_id','goal_name','goal_target'])# the name is a string
with self.sm_GiveBack:
# rospy.logerr('sm_GiveBack add State')
# self.sm_GiveBack.userdata.nav_ps = self.waypoints[0]
# self.sm_GiveBack.userdata.nav_mode_1 =0
# StateMachine.add('NAV_ROOM',
# NavStack(),
# remapping ={'pos_xm':'nav_ps','mode':'nav_mode_1'},
# transitions ={'succeeded':'FACE_RECO','aborted':'NAV_ROOM','error':'error'})
# self.sm_GiveBack.userdata.name_id =0
# self.sm_GiveBack.userdata.name_list =list()
self.sm_GiveBack.userdata.sentences = "please look at me"
StateMachine.add('SPEAK',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'WAIT','aborted':'aborted','error':'error'})
self.sm_GiveBack.userdata.rec =5.0
StateMachine.add('WAIT',
Wait(),
remapping ={'rec':'rec'},
transitions ={'succeeded':'FACE_RECO','error':'error'})
self.sm_GiveBack.userdata.person_position =PointStamped()
StateMachine.add('FACE_RECO',
FaceReco(),
remapping ={'position':'person_position','name_id':'name_id'},
transitions ={'succeeded':'NAV_GO',
'again':'FACE_RECO',
'aborted':'FACE_RECO',
'error':'error',
'turn_l':'TURN_L',
'turn_r':'TURN_R',
'train_error':'aborted'}
)
self.sm_GiveBack.userdata.turn_point_1 = Point(0.0,0.0,pi/8)
StateMachine.add('TURN_L',
SimpleMove_move(),
transitions={'succeeded':'SPEAK_2','error':'error'},
remapping ={'point':'turn_point_1'})
self.sm_GiveBack.userdata.turn_point_2 = Point(0.0,0.0,-pi/8)
StateMachine.add('TURN_R',
SimpleMove_move(),
remapping ={'point':'turn_point_2'},
transitions ={'succeeded':'SPEAK_2','error':'error'})
StateMachine.add('SPEAK_2',
Speak(),
remapping ={'sentences':'sentences'},
transitions ={'succeeded':'FACE_RECO','aborted':'aborted','error':'error'})
#please pay attention!
# self.sm_GiveBack.userdata.nav_mode_2 =1
StateMachine.add('NAV_GO',
NavStack(),
remapping ={'pos_xm':'person_position'},
transitions ={'succeeded':'GET_NAME','aborted':'NAV_GO','error':'error'})
StateMachine.add("GET_NAME",
NameHehe2(),
remapping ={'goal_target':'goal_name','goal_name':'goal_name','goal_target':'goal_target', 'sentences':'sentences_1'},
transitions ={'succeeded':'TALK_1','aborted':'aborted','error':'error'})
self.sm_GiveBack.userdata.sentences_1 =''
StateMachine.add('TALK_1',
Speak(),
remapping ={'sentences':"sentences_1"},
transitions ={'succeeded':'PUT','aborted':"PUT",'error':'error'})
StateMachine.add('PUT' , PlaceBag(),
transitions = {'succeeded':'succeeded',
'aborted':'succeeded'})
# this pose should be specified to make the people get it
# self.sm_GiveBack.userdata.arm_mode =2
# self.sm_GiveBack.userdata.place_ps = PointStamped()
# self.sm_GiveBack.userdata.place_ps.header.frame_id ='base_footprint'
# self.sm_GiveBack.userdata.place_ps.point.x =0.7
# self.sm_GiveBack.userdata.place_ps.point.y =0.0
# self.sm_GiveBack.userdata.place_ps.point.z =0.3
# StateMachine.add('PLACE',
# ArmCmd(),
# remapping ={'mode':'arm_mode','arm_ps':'place_ps'},
# transitions ={'succeeded':'TALK_2','aborted':'PLACE','error':'error'})
# self.sm_GiveBack.userdata.sentences_2 = 'I get the thing you want, am I?'
# StateMachine.add('TALK_2',
# Speak(),
# remapping ={'sentences':'sentences_2'},
# transitions ={'succeeded':'succeeded','aborted':"aborted",'error':'error'})
self.sm_EndTask = StateMachine(outcomes =['succeeded','aborted','error'])
with self.sm_EndTask:
# rospy.logerr('sm_EndTask add State')
self.sm_EndTask.userdata.nav_ps = self.waypoints[2]
# self.sm_EndTask.userdata.nav_mode = 0
StateMachine.add('NAV_BYEBYE',
NavStack(),
remapping ={'pos_xm':'nav_ps'},
transitions ={'succeeded':'succeeded','aborted':'NAV_BYEBYE','error':'error'})
self.sm_EndTask.userdata.point = Point(1.8,0.0,0.0)
# StateMachine.add('SIMPLE_MOVE',
# SimpleMove_move(),
# remapping={'point':'point'},
# transitions={'succeeded':'succeeded','aborted':'aborted','error':'error'})
self.WHOISWHO = StateMachine(outcomes =['succeeded','aborted','error'])
with self.WHOISWHO:
# we can generate some state which contant the list_value
StateMachine.add('ENTERROOM',
self.sm_EnterRoom,
transitions ={'succeeded':'FACEDETECT','aborted':'aborted','error':'error'})
self.WHOISWHO.userdata.people_position =list()
self.WHOISWHO.userdata.num_list = list()
StateMachine.add('FACEDETECT',
self.sm_FaceDetect,
remapping ={'people_position':'people_position','num_list':'num_list'},
transitions = {'succeeded':'GETPERSON','aborted':'aborted','error':'error'}
)
self.WHOISWHO.userdata.person_position = PointStamped()
# if someone is recongized failed, we should make the total list in the same order
# get the last element of the list
# FGM: This state is to get different people pos
StateMachine.add('GETPERSON',
GetValue(),
remapping ={'element_list':'people_position','element':'person_position'},
transitions ={'succeeded':'REMEMBER','aborted':"GETID",'error':'error'}
)
# the cv will remember the face with the increasing ID
self.WHOISWHO.userdata.name =''
self.WHOISWHO.userdata.target= ''
self.WHOISWHO.userdata.name_list =list()
self.WHOISWHO.userdata.target_list =list()
self.WHOISWHO.userdata.goal_name =''
self.WHOISWHO.userdata.goal_target''
# if donnot need to remember, the order of ids should be the same with the positions return
# the first of the position should be the 0
# last be the 4
StateMachine.add('REMEMBER',
self.sm_Remember,
remapping ={'person_position':'person_position','name':'name','target':'target'},
transitions ={'succeeded':'NAMEINLIST','aborted':'aborted','error':'error'}
)
#this state use for joining the name and the target into the name_list and target_list
# insert in the head of the lists
# if the name and target are empty, we should also append them to the lists
StateMachine.add('NAMEINLIST',
NameInList(),
remapping ={'name':'name','target':'target','name_list':'name_list','target_list':'target_list'},
transitions ={'succeeded':'GETPERSON','aborted':'aborted','error':'error'}
)
# ############################################################################
# so far here ,the tasks of remember is completed , the rest is the tasks to return the target to the people
# we should take out the name and the matched target for xm to grasp and give back
# in fact, we will pop the name and target out the list,so the name_id is gradually reduced
# ############################################################################
StateMachine.add('GETTARGET',
GetValue2(),
remapping ={'element_list':'target_list','element':'target'},
transitions ={'succeeded':'CATCHTARGET','aborted':'ENDTASK','error':'error'})
StateMachine.add('CATCHTARGET',
self.sm_GetTarget,
transitions= {'succeeded':'NAV_ROOM','aborted':'NAV_ROOM','error':'error'},
remapping = {'target':'target'})
# ############################################################################
self.WHOISWHO.userdata.nav_ps = self.waypoints[0]
# self.WHOISWHO.userdata.nav_mode_1 =0
StateMachine.add('NAV_ROOM',
NavStack(),
remapping ={'pos_xm':'nav_ps'},
transitions ={'succeeded':'GIVEBACK','aborted':'NAV_ROOM','error':'error'})
StateMachine.add('CHECKFINISH',
CBState(self.checkfinish,outcomes=['finish','continue'],input_keys=['num_list']),
transitions={'finish':'ENDTASK',
'continue':'GETTARGET'},
remapping={'num_list':'num_list'})
# StateMachine.add('CLEAR_MAP',
# ClearMap(),
# transitions ={'succeeded':'GETID','aborted':'GETID'})
# get the last num of the list, ID
# if the name and target list is empty, will skipped this item
self.WHOISWHO.userdata.sentences_1 = 'please change the order'
StateMachine.add('SPEAK_1',
Speak(),
remapping ={'sentences':'sentences_1'},
transitions ={'succeeded':'WAIT_HEHE','aborted':'WAIT_HEHE','error':'error'})
self.WHOISWHO.userdata.rec_hehe =10.0
StateMachine.add('WAIT_HEHE',
Wait(),
remapping ={'rec':'rec_hehe'},
transitions ={'succeeded':'SPEAK_2','error':'error'})
self.WHOISWHO.userdata.sentences_2 = 'I will make the recongization task'
StateMachine.add('SPEAK_2',
Speak(),
transitions ={'succeeded':'GETID','aborted':'GETID','error':'error'},
remapping ={'sentences':'sentences_2'})
self.WHOISWHO.userdata.name_id = 0
StateMachine.add('GETID',
GetId(),
remapping ={'output_id':'name_id','input_list':'name_list','num_list':'num_list'},
transitions ={'succeeded':'GETTARGET','aborted':'ENDTASK','error':'error'}
)
StateMachine.add('GIVEBACK',
self.sm_GiveBack,
remapping ={'name_id':'name_id','name_list':'name_list','target_list':'target_list'},
transitions ={'succeeded':'CHECKFINISH','aborted':'CHECKFINISH','error':'error'}
)
StateMachine.add('ENDTASK',
self.sm_EndTask,
transitions ={'succeeded':'succeeded','aborted':'aborted','error':'error'})
# rospy.logerr('sm_Top execute begin')
intro_server = IntrospectionServer('whoiswho',self.WHOISWHO, 'SM_ROOT')
intro_server.start()
out = self.WHOISWHO.execute()
intro_server.stop()
if out == 'succeeded':
self.smach_bool = True
if __name__ == '__main__':
rospy.init_node('drone_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome'])
sm.userdata.lspeed = 0.5
sm.userdata.rspeed = 1.2
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('drone_server', sm, '/SM_DRONE')
sis.start()
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('TAKEOFF', CBState(takeoff_cb),
{'finished': 'MOVE', 'failed':'outcome'})
smach.StateMachine.add('MOVE', CBState(move_cb),
{'finished': 'ROTATE', 'failed':'outcome'})
smach.StateMachine.add('ROTATE', CBState(rotate_cb),
{'finished': 'LAND', 'failed':'outcome'})
smach.StateMachine.add('LAND', CBState(land_cb),
{'finished': 'outcome', 'failed':'outcome'})
# Execute SMACH plan
outcome = sm.execute()
sis.stop()
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'})
#else:
#return 'failed'
### -- BUCLE PRINCIPAL -- ###
if __name__ == '__main__':
rospy.init_node('altern_tripod_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome'])
# Open the container
with sm:
# Stand UP state
smach.StateMachine.add('ON_FLOOR', CBState(on_floor_cb), {
'finished': 'STOP',
'failed': 'outcome'
})
# Stop state
smach.StateMachine.add(
'STOP', CBState(stop_cb), {
'finished_1': 'ALTERN_TRIPOD',
'finished_2': 'TETRAPOD',
'finished_3': 'STAIR_CLIMB',
'failed': 'outcome'
})
# Altern tripod state
smach.StateMachine.add('ALTERN_TRIPOD', CBState(altern_tripod_cb), {