motiondata = rfpm.play_and_record(motion_name, bag_name = motion_name)
rospy.loginfo("Motion played and recorded, there should be a bag named: " + motion_name + ".bag")
rospy.loginfo("The data for this bag is: " + str(motiondata))
# Looks like:
# {'joints': ['head_2_joint'],
# 'rosbag_name': 'yes.bag',
# 'motion_name': 'yes'}
joint_names = motiondata['joints']
rospy.loginfo("Preparing other nodes...")
# Get publisher for displaying generated trajectory TODO: add this to the class itself
pub = rospy.Publisher('/move_group/display_planned_path', DisplayTrajectory)
# Start gesture generator
gG = gestureGeneration()
# Start gesture executor
ge = gestureExecution()
# Start plotter
dp = dmpPlanTrajectoryPlotter()
rospy.loginfo("Nodes initialized.")
sound_pub.publish(createSoundGoal("Now I'm going to learn from the trajectory recorded, I'll need some seconds"))
rospy.loginfo("Now we will generate a DMP from this bag.")
#gesture_dict = gG.loadGestureFromBagJointStates(motion_name +".bag", joint_names)
gesture_dict = gG.loadGestureFromBagJointStatesAndRemoveJerkiness(motion_name +".bag", joint_names)
#gesture_dict = gG.loadGestureFromBagJointStatesAndDownsample(motion_name +".bag", joint_names)
copy_gest_dict = copy.deepcopy(gesture_dict)
del copy_gest_dict['computed_dmp']
rospy.loginfo("Now we got a DMP and a gesture dictionary containing it, looks like (omitting computed_dmp): " + str(copy_gest_dict))
rospy.loginfo("Motion played and recorded, there should be a bag named: " +
motion_name + ".bag")
rospy.loginfo("The data for this bag is: " + str(motiondata))
# Looks like:
# {'joints': ['head_2_joint'],
# 'rosbag_name': 'yes.bag',
# 'motion_name': 'yes'}
joint_names = motiondata['joints']
rospy.loginfo("Preparing other nodes...")
# Get publisher for displaying generated trajectory TODO: add this to the class itself
pub = rospy.Publisher('/move_group/display_planned_path',
DisplayTrajectory)
# Start gesture generator
gG = gestureGeneration()
# Start gesture executor
ge = gestureExecution()
# Start plotter
dp = dmpPlanTrajectoryPlotter()
rospy.loginfo("Nodes initialized.")
sound_pub.publish(
createSoundGoal(
"Now I'm going to learn from the trajectory recorded, I'll need some seconds"
))
rospy.loginfo("Now we will generate a DMP from this bag.")
#gesture_dict = gG.loadGestureFromBagJointStates(motion_name +".bag", joint_names)
gesture_dict = gG.loadGestureFromBagJointStatesAndRemoveJerkiness(
motion_name + ".bag", joint_names)
#gesture_dict = gG.loadGestureFromBagJointStatesAndDownsample(motion_name +".bag", joint_names)
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
self.lfjs = LearnFromJointState()
self.gesture_generator = gestureGeneration()
self.gesture_executor = gestureExecution()
# smach.StateMachine.add('ActivateKWASR_learn_gesture',
# ActivateKeywordASR(keyword=LEARN_GESTURE_SENTENCE),
# transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'Explain_how_it_works',
text_to_say("Hello, you will teach me a new gesture.\
First tell me which arms and hands to use. Remember, I only have two!", wait=False),
transitions={'succeeded': 'listen_groups_to_use', 'aborted': 'aborted'})
smach.StateMachine.add(
'listen_groups_to_use',
ListenRepeatSM(grammar=CHOOSE_GROUP_GRAMMAR_NAME),
transitions={'succeeded': 'confirm_groups', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_groups',
SayYesOrNoSM_2(),
transitions={'succeeded': 'prepare_groups_to_make_loose', 'aborted': 'listen_groups_to_use',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepare_groups_to_make_loose',
prepareGroups(),
transitions={'succeeded': 'Warn_loose_joints', 'aborted': 'aborted'})
smach.StateMachine.add(
'Warn_loose_joints',
text_to_say("I'm going to lower the current on my joints, be careful.", wait=False),
transitions={'succeeded': 'look_to_place', 'aborted': 'aborted'})
# def look_point_goal_cb(userdata, goal):
# phg = PointHeadGoal()
# phg.min_duration = rospy.Duration(2.0)
# phg.pointing_axis.x = 1.0
# phg.pointing_frame = "stereo_link"
# phg.target.header.frame_id = "base_link"
# phg.target.point.x = 1.0
# phg.target.point.y = -1.0
# phg.target.point.z = 1.0
# return phg
#
# smach.StateMachine.add(
# 'look_to_place',
# SimpleActionState('/head_controller/point_head_action',
# PointHeadAction,
# goal_cb=look_point_goal_cb),
# transitions={'succeeded': 'make_groups_loose', 'aborted': 'aborted'})
smach.StateMachine.add(
'look_to_place',
sendPointHeadGoalByPublisher(Point(1.0, -1.0, 1.0)),
transitions={'succeeded': 'make_groups_loose', 'aborted': 'aborted'})
smach.StateMachine.add(
'make_groups_loose',
setJointsCurrent(curr_limit=0.01),
transitions={'succeeded': 'ActivateKWASR_start', 'aborted': 'aborted'})
smach.StateMachine.add('ActivateKWASR_start',
ActivateKeywordASR(keyword=START_SENTENCE),
transitions={'succeeded': 'Say_ready', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'Say_ready',
text_to_say("Please say, start, when you want to start recording. Then say, wait here, when you finish the gesture."),
transitions={'succeeded': 'listen_to_start', 'aborted': 'aborted'})
smach.StateMachine.add(
'listen_to_start',
ReadASR(), # We can only listen to one word, so we dont care about the content
transitions={'succeeded': 'ActivateKWASR_stop', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add('ActivateKWASR_stop',
ActivateKeywordASR(keyword=STOP_SENTENCE),
transitions={'succeeded': 'Say_recording', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'Say_recording',
text_to_say("Ok, I'll start recording... now."),
transitions={'succeeded': 'record_gesture', 'aborted': 'aborted'})
smach.StateMachine.add(
'record_gesture',
startRecordGesture(self.lfjs),
transitions={'succeeded': 'listen_to_stop', 'aborted': 'aborted'})
smach.StateMachine.add(
'listen_to_stop',
ReadASR(),
transitions={'succeeded': 'stop_recording_gesture', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'stop_recording_gesture',
stopRecordGesture(self.lfjs),
transitions={'succeeded': 'Say_recorded_and_processing', 'aborted': 'aborted'})
smach.StateMachine.add(
'Say_recorded_and_processing',
text_to_say("Got the gesture, now I'm going to process it. Give me some seconds please.", wait=False),
transitions={'succeeded': 'look_to_place_middle', 'aborted': 'aborted'})
# def look_point_goal_cb2(userdata, goal):
# phg = PointHeadGoal()
# phg.min_duration = rospy.Duration(2.0)
# phg.pointing_axis.x = 1.0
# phg.pointing_frame = "stereo_link"
# phg.target.header.frame_id = "base_link"
# phg.target.point.x = 1.0
# phg.target.point.y = 0.0
# phg.target.point.z = 1.7
# return phg
#
# smach.StateMachine.add(
# 'look_to_place_middle',
# SimpleActionState('/head_controller/point_head_action',
# PointHeadAction,
# goal_cb=look_point_goal_cb2),
# transitions={'succeeded': 'train_gesture', 'aborted': 'aborted'})
smach.StateMachine.add(
'look_to_place_middle',
sendPointHeadGoalByPublisher(Point(1.0, 0.0, 1.7)),
transitions={'succeeded': 'train_gesture', 'aborted': 'aborted'})
smach.StateMachine.add(
'train_gesture',
gestureTrainer(self.gesture_generator),
transitions={'succeeded': 'Say_gesture_learnt', 'aborted': 'aborted'})
smach.StateMachine.add(
'Say_gesture_learnt',
text_to_say("Ok, I learnt the gesture, as Neeo would say, now I know kung fu. \
Put my arm where you want me to start the motion and say now."),
transitions={'succeeded': 'ActivateKWASR_now', 'aborted': 'aborted'})
smach.StateMachine.add('ActivateKWASR_now',
ActivateKeywordASR(keyword=EXECUTE_SENTENCE),
transitions={'succeeded': 'listen_to_now', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'listen_to_now',
ReadASR(),
transitions={'succeeded': 'make_group_stiff', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'make_group_stiff',
setJointsCurrent(curr_limit=1.0),
transitions={'succeeded': 'Say_executing', 'aborted': 'aborted'})
smach.StateMachine.add(
'Say_executing',
text_to_say("Ok, I'm going to execute the gesture from my current configuration!", wait=False),
transitions={'succeeded': 'execute_gesture_from_current_pose', 'aborted': 'aborted'})
smach.StateMachine.add(
'execute_gesture_from_current_pose',
gestureExecuterFromPlace(self.gesture_executor, self.gesture_generator),
transitions={'succeeded': 'Say_try_from_another_position', 'aborted': 'Say_fail_exec'})
smach.StateMachine.add(
'Say_try_from_another_position',
text_to_say("That went well, try again from a different position. I'm making my joints loose again. When you want me to execute again the gesture say now."),
transitions={'succeeded': 'make_groups_loose_again', 'aborted': 'aborted'})
smach.StateMachine.add(
'Say_fail_exec',
text_to_say("The safety check of this movement failed, try again from a safer position. Making my joints loose again, say now when you want to execute."),
transitions={'succeeded': 'make_groups_loose_again', 'aborted': 'aborted'})
smach.StateMachine.add(
'make_groups_loose_again',
setJointsCurrent(curr_limit=0.01),
transitions={'succeeded': 'ActivateKWASR_now', 'aborted': 'aborted'})