def __init__(self): self.dm = DragonbotManager() self.dm.pose_off() self.exp_sub = rospy.Subscriber("/dragon_teleop_GUI/expressions", String, self.exp_callback) self.mot_sub = rospy.Subscriber("/dragon_teleop_GUI/motions", String, self.mot_callback) self.ph_sub = rospy.Subscriber("/dragon_teleop_GUI/phrases", String, self.ph_callback) self.pose_on_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_on", String, self.pos_on_cb) self.x_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_x", Int32, self.x_pos_cb) self.y_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_y", Int32, self.y_pos_cb) self.z_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_z", Int32, self.z_pos_cb) self.look_on_sub = rospy.Subscriber("/dragon_teleop_GUI/lookat_on", String, self.look_on_cb) self.xl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_x", Int32, self.x_look_cb) self.yl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_y", Int32, self.y_look_cb) self.zl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_z", Int32, self.z_look_cb) self.blink_sub = rospy.Subscriber("/dragon_teleop_GUI/blink", String, self.blink_cb) rospy.loginfo("Ready!") self.current_pose = [0,0,0] self.pose_on = False self.x_min = -2.3 self.x_max = 2.5 self.x_range = self.x_max - self.x_min self.y_min = -2.49 self.y_max = 3.4 self.y_range = self.y_max - self.y_min self.z_min = -2.0 self.z_max = 2.6 self.z_range = self.z_max - self.z_min self.v = 1 self.a = .05 self.current_look = [0,0,20] self.look_on = False self.x_lmin = -300 self.x_lmax = 300 self.x_lrange = self.x_lmax - self.x_lmin self.y_lmin = -300 self.y_lmax = 300 self.y_lrange = self.y_lmax - self.y_lmin self.z_lmin = 20 self.z_lmax = 400 self.z_lrange = self.z_lmax - self.z_lmin
def main(): rospy.init_node('intro_controller') dm = DragonbotManager() tm = TabletManager() rospy.loginfo("loading dialogue") with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/intro_dialogue.yaml", 'r') as f: s = f.read() dialogue_name = "intro" dialogue = yaml.load(s) dg = DialogueManager(tm, dialogue_name, dialogue[dialogue_name]) dm.eye_close() tm.change("sleep") while not rospy.is_shutdown() and not tm.last_press("/dragon_GUI/sleep") == 0: dm.say("intro-05-snore_sleep", wait = False) rospy.sleep(6.0) dm.express("wakeup") dm.eye_open() dg.play_dialogue("intro_dialogue") dm.eye_close() while not rospy.is_shutdown(): dm.say("intro-05-snore_sleep", wait = False) rospy.sleep(6.0)
def main(): dm = DragonbotManager() tm = TabletManager() with open("parenting_center_dialogue.yaml") as f: s = f.read() dialogues = yaml.load(s) dg = DialogueManager(dm, tm, "parenting_center_dialogue", dialogues["parenting_center_dialogue"]) dm.eye_close() tm.change("sleep") while not rospy.is_shutdown() and not tm.last_press("/dragon_GUI/sleep") == 1: dm.say("intro-05-snore_sleep", wait = False) rospy.sleep(6.0) dm.express("surprised") dm.eye_open() try: dg.play_dialogue("introduction_parenting_center") except PanicException: dm.eye_close() tm.change("sleep") return 'AHHH PANIC! SOMETHING BAD HAPPENED!' except NextStateException: return 'I want to end the dialogue, but nothing bad happened' except NextPhraseException: # Ignore this pass # When we get to the end of the dialogue, Chilly goes back to sleep dm.eye_close() tm.change("sleep") return "Finished dialogue successfully"
class DragonTeleop(): def __init__(self): self.dm = DragonbotManager() self.dm.pose_off() self.exp_sub = rospy.Subscriber("/dragon_teleop_GUI/expressions", String, self.exp_callback) self.mot_sub = rospy.Subscriber("/dragon_teleop_GUI/motions", String, self.mot_callback) self.ph_sub = rospy.Subscriber("/dragon_teleop_GUI/phrases", String, self.ph_callback) self.pose_on_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_on", String, self.pos_on_cb) self.x_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_x", Int32, self.x_pos_cb) self.y_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_y", Int32, self.y_pos_cb) self.z_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_z", Int32, self.z_pos_cb) self.look_on_sub = rospy.Subscriber("/dragon_teleop_GUI/lookat_on", String, self.look_on_cb) self.xl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_x", Int32, self.x_look_cb) self.yl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_y", Int32, self.y_look_cb) self.zl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_z", Int32, self.z_look_cb) self.blink_sub = rospy.Subscriber("/dragon_teleop_GUI/blink", String, self.blink_cb) rospy.loginfo("Ready!") self.current_pose = [0,0,0] self.pose_on = False self.x_min = -2.3 self.x_max = 2.5 self.x_range = self.x_max - self.x_min self.y_min = -2.49 self.y_max = 3.4 self.y_range = self.y_max - self.y_min self.z_min = -2.0 self.z_max = 2.6 self.z_range = self.z_max - self.z_min self.v = 1 self.a = .05 self.current_look = [0,0,20] self.look_on = False self.x_lmin = -300 self.x_lmax = 300 self.x_lrange = self.x_lmax - self.x_lmin self.y_lmin = -300 self.y_lmax = 300 self.y_lrange = self.y_lmax - self.y_lmin self.z_lmin = 20 self.z_lmax = 400 self.z_lrange = self.z_lmax - self.z_lmin def exp_callback(self, data): rospy.loginfo("Got expression: " + data.data) self.dm.express(data.data) def mot_callback(self, data): rospy.loginfo("Got motion: " + data.data) self.dm.express(data.data) def ph_callback(self, data): rospy.loginfo("Got phrase: " + data.data) self.dm.say(data.data) def blink_cb(self, data): rospy.loginfo("Got blink command: " + data.data) if data.data == "once": self.dm.blink() elif data.data == "hold": self.dm.eye_close() elif data.data == "hold_off": self.dm.eye_open() def pos_on_cb(self, data): rospy.loginfo("Setting pose to: " + data.data) if data.data == "on": self.current_pose = [0,0,0] self.dm.pose(0,0,0) self.pose_on = True elif data.data == " off": self.pose_on = False self.dm.pose_off() def x_pos_cb(self, data): rospy.loginfo("Got x pose value: " + str(data.data)) if self.pose_on: self.current_pose[0] = self.x_min + (float(data.data)/100) * self.x_range rospy.loginfo("Setting pose to: " + str(self.current_pose)) self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a) def y_pos_cb(self, data): rospy.loginfo("Got y pose value: " + str(data.data)) if self.pose_on: self.current_pose[1] = self.y_min + (float(data.data)/100) * self.y_range rospy.loginfo("Setting pose to: " + str(self.current_pose)) self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a) def z_pos_cb(self, data): rospy.loginfo("Got z pose value: " + str(data.data)) if self.pose_on: self.current_pose[2] = self.z_min + (float(data.data)/100) * self.z_range rospy.loginfo("Setting pose to: " + str(self.current_pose)) self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a) def look_on_cb(self, data): rospy.loginfo("Setting lookat to: " + data.data) if data.data == "on": self.current_look = [0,0,20] self.dm.lookat(0,0,20) self.look_on = True elif data.data == " off": self.look_on = False self.dm.lookat_off() def x_look_cb(self, data): rospy.loginfo("Got x lookat value: " + str(data.data)) if self.look_on: self.current_look[0] = self.x_lmin + (float(data.data)/100) * self.x_lrange rospy.loginfo("Setting lookat to: " + str(self.current_look)) self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2]) def y_look_cb(self, data): rospy.loginfo("Got y lookat value: " + str(data.data)) if self.look_on: self.current_look[1] = self.y_lmin + (float(data.data)/100) * self.y_lrange rospy.loginfo("Setting lookat to: " + str(self.current_look)) self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2]) def z_look_cb(self, data): rospy.loginfo("Got z lookat value: " + str(data.data)) if self.look_on: self.current_look[2] = self.z_lmin + (float(data.data)/100) * self.z_lrange rospy.loginfo("Setting lookat to: " + str(self.current_look)) self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2])
def main(): rospy.init_node('experiment_controller') if not rospy.has_param("~max_time"): rospy.set_param("~max_time", 600) rospy.set_param("~start_time", rospy.Time.now().secs) day = rospy.get_param("~lesson") day_num = rospy.get_param("~day") sm = smach.StateMachine(outcomes=['end']) lesson_list = {'lunchbox':("whole_grains", "drinks"), 'snacks':("snacks1", "snacks2"), 'breakfast':("cereal", "breakfast"), 'dinner':("sides1","sides2")} dm = DragonbotManager() dm.eye_open() tm = TabletManager() #info: day, lessons lessons = lesson_list[day] day = day + str(day_num) info = (day,lessons) rospy.loginfo("Reading food phrases file.") if day_num == 1: with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/day1_food_phrases.yaml", 'r') as f: s = f.read() elif day_num == 2: with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/day2_food_phrases.yaml", 'r') as f: s = f.read() else: print "Usage: day number must be 1 or 2" sys.exit() # file format is: # lesson: # intro: phrase_id # reminder: phrase_idfg # no_choice: phrase_id # foodname: (food chosen before) # foodname: phrase_id (food chosen after) # index as food_info["lesson"]["choice1"]["choice2"] # nb: food_info["lesson"]["none"].keys() will give all foods (and "none") food_info = yaml.load(s) rospy.loginfo("Reading other dialogue phrases file.") with open(roslib.packages.get_pkg_dir("expeditions_year1")+ "/yaml/dialogue_specification.yaml", 'r') as f: s = f.read() # file format is: dialogue_info = yaml.load(s) #rospy.loginfo("Loading phrase information file.") #dm.load_phrases("phrases.yaml") #rospy.loginfo("Done loading files.") if day_num == 1: food_state = FoodChoiceDay1(dm, tm, info, dialogue_info[day + "_foods"],food_info) elif day_num == 2: food_state = FoodChoiceDay2(dm, tm, info, dialogue_info[day + "_foods"],food_info) with sm: smach.StateMachine.add('WORKOUT', Workout(dm, tm, info, dialogue_info[day + "_workout"]), transitions={'panic':'SLEEP', 'continue':'WORKOUT', 'end':'OUTRO', 'timeout':'OUTRO'}) smach.StateMachine.add('SLEEP', Sleep(dm, tm, info), transitions={'wakeup':'INTRO', 'done':'end'}) smach.StateMachine.add('INTRO', Intro(dm, tm, info, dialogue_info[day + "_intro"]), transitions={'panic':'SLEEP', 'end':'F_CHOICE'}) smach.StateMachine.add('F_CHOICE', food_state, transitions={'panic':'SLEEP', 'next_round':'F_CHOICE', 'end':'WORKOUT', 'timeout':'OUTRO'}) smach.StateMachine.add('OUTRO', Outro(dm, tm, info, dialogue_info[day + "_outro"]), transitions={'end':'SLEEP', 'panic':'SLEEP'}) outcome = sm.execute()